123 lines
4.6 KiB
C++
123 lines
4.6 KiB
C++
// SPDX-FileCopyrightText: 2021 Phillip Burgess for Adafruit Industries
|
|
//
|
|
// SPDX-License-Identifier: MIT
|
|
|
|
#include <bluefruit.h>
|
|
|
|
// packetbuffer holds inbound data
|
|
#define READ_BUFSIZE 20
|
|
uint8_t packetbuffer[READ_BUFSIZE + 1]; // +1 is for NUL string terminator
|
|
|
|
/**************************************************************************/
|
|
/*!
|
|
@brief Casts the four bytes at the specified address to a float.
|
|
@param ptr Pointer into packet buffer.
|
|
@returns Floating-point value.
|
|
*/
|
|
/**************************************************************************/
|
|
float parsefloat(uint8_t *ptr) {
|
|
float f; // Make a suitably-aligned float variable,
|
|
memcpy(&f, ptr, 4); // because in-buffer instance might be misaligned!
|
|
return f; // (You can't always safely parse in-place)
|
|
}
|
|
|
|
/**************************************************************************/
|
|
/*!
|
|
@brief Prints a series of bytes in 0xNN hexadecimal notation.
|
|
@param buf Pointer to array of byte data.
|
|
@param len Data length in bytes.
|
|
*/
|
|
/**************************************************************************/
|
|
void printHex(const uint8_t *buf, const uint32_t len) {
|
|
for (uint32_t i=0; i < len; i++) {
|
|
Serial.print(F("0x"));
|
|
if (buf[i] <= 0xF) Serial.write('0'); // Zero-pad small values
|
|
Serial.print(buf[i], HEX);
|
|
if (i < (len - 1)) Serial.write(' '); // Space between bytes
|
|
}
|
|
Serial.println();
|
|
}
|
|
|
|
static const struct { // Special payloads from Bluefruit Connect app...
|
|
char id; // Packet type identifier
|
|
uint8_t len; // Size of complete, well-formed packet of this type
|
|
} _app_packet[] = {
|
|
{'A', 15}, // Accelerometer
|
|
{'G', 15}, // Gyro
|
|
{'M', 15}, // Magnetometer
|
|
{'Q', 19}, // Quaterion
|
|
{'B', 5}, // Button
|
|
{'C', 6}, // Color
|
|
{'L', 15}, // Location
|
|
};
|
|
#define NUM_PACKET_TYPES (sizeof _app_packet / sizeof _app_packet[0])
|
|
#define SHORTEST_PACKET_LEN 5 // Button, for now
|
|
|
|
/**************************************************************************/
|
|
/*!
|
|
@brief Given packet data, identify if it's one of the known
|
|
Bluefruit Connect app packet types.
|
|
@param buf Pointer to packet data.
|
|
@param len Size of packet in bytes.
|
|
@returns Packet type index (0 to NUM_PACKET_TYPES-1) if recognized,
|
|
-1 if unrecognized.
|
|
*/
|
|
/**************************************************************************/
|
|
int8_t packetType(uint8_t *buf, uint8_t len) {
|
|
if ((len >= SHORTEST_PACKET_LEN) && (buf[0] == '!')) {
|
|
for (int8_t type=0; type<NUM_PACKET_TYPES; type++) {
|
|
if ((buf[1] == _app_packet[type].id) &&
|
|
(len == _app_packet[type].len)) {
|
|
return type;
|
|
}
|
|
}
|
|
}
|
|
return -1; // Length too short for a packet, or not a recognized type
|
|
}
|
|
|
|
/**************************************************************************/
|
|
/*!
|
|
@brief Wait for incoming data and determine if it's one of the
|
|
special Bluefruit Connect app packet types.
|
|
@param ble Pointer to BLE UART object.
|
|
timeout Character read timeout in milliseconds.
|
|
@returns Length of data, or 0 if checksum is invalid for the type of
|
|
packet detected.
|
|
@note Packet buffer is not cleared. Calling function is expected
|
|
to check return value before deciding whether to act on the
|
|
data.
|
|
*/
|
|
/**************************************************************************/
|
|
uint8_t readPacket(BLEUart *ble, uint16_t timeout) {
|
|
int8_t type = -1; // App packet type, -1 if unknown or freeform string
|
|
uint8_t len = 0, xsum = 255; // Packet length and ~checksum so far
|
|
uint32_t now, start_time = millis();
|
|
do {
|
|
now = millis();
|
|
if (ble->available()) {
|
|
char c = ble->read();
|
|
if (c == '!') { // '!' resets buffer to start
|
|
len = 0;
|
|
xsum = 255;
|
|
}
|
|
packetbuffer[len++] = c;
|
|
// Stop when buffer's full or packet type recognized
|
|
if ((len >= READ_BUFSIZE) ||
|
|
((type = packetType(packetbuffer, len)) >= 0)) break;
|
|
start_time = now; // Reset timeout on char received
|
|
xsum -= c; // Not last char, do checksum math
|
|
type = -1; // Reset packet type finder
|
|
}
|
|
} while((now - start_time) < timeout);
|
|
|
|
// If packet type recognized, verify checksum (else freeform string)
|
|
if ((type >= 0) && (xsum != packetbuffer[len-1])) { // Last byte = checksum
|
|
Serial.print("Packet checksum mismatch: ");
|
|
printHex(packetbuffer, len);
|
|
return 0;
|
|
}
|
|
|
|
packetbuffer[len] = 0; // Terminate packet string
|
|
|
|
return len; // Checksum is valid for packet, or it's a freeform string
|
|
}
|