You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
135 lines
3.5 KiB
135 lines
3.5 KiB
6 years ago
|
#include <string.h>
|
||
|
#include <Arduino.h>
|
||
|
#include <bluefruit.h>
|
||
|
|
||
|
|
||
|
#define PACKET_ACC_LEN (15)
|
||
|
#define PACKET_GYRO_LEN (15)
|
||
|
#define PACKET_MAG_LEN (15)
|
||
|
#define PACKET_QUAT_LEN (19)
|
||
|
#define PACKET_BUTTON_LEN (5)
|
||
|
#define PACKET_COLOR_LEN (6)
|
||
|
#define PACKET_LOCATION_LEN (15)
|
||
|
|
||
|
// READ_BUFSIZE Size of the read buffer for incoming packets
|
||
|
#define READ_BUFSIZE (20)
|
||
|
|
||
|
|
||
|
/* Buffer to hold incoming characters */
|
||
|
uint8_t packetbuffer[READ_BUFSIZE+1];
|
||
|
|
||
|
/**************************************************************************/
|
||
|
/*!
|
||
|
@brief Casts the four bytes at the specified address to a float
|
||
|
*/
|
||
|
/**************************************************************************/
|
||
|
float parsefloat(uint8_t *buffer)
|
||
|
{
|
||
|
float f;
|
||
|
memcpy(&f, buffer, 4);
|
||
|
return f;
|
||
|
}
|
||
|
|
||
|
/**************************************************************************/
|
||
|
/*!
|
||
|
@brief Prints a hexadecimal value in plain characters
|
||
|
@param data Pointer to the byte data
|
||
|
@param numBytes Data length in bytes
|
||
|
*/
|
||
|
/**************************************************************************/
|
||
|
void printHex(const uint8_t * data, const uint32_t numBytes)
|
||
|
{
|
||
|
uint32_t szPos;
|
||
|
for (szPos=0; szPos < numBytes; szPos++)
|
||
|
{
|
||
|
Serial.print(F("0x"));
|
||
|
// Append leading 0 for small values
|
||
|
if (data[szPos] <= 0xF)
|
||
|
{
|
||
|
Serial.print(F("0"));
|
||
|
Serial.print(data[szPos] & 0xf, HEX);
|
||
|
}
|
||
|
else
|
||
|
{
|
||
|
Serial.print(data[szPos] & 0xff, HEX);
|
||
|
}
|
||
|
// Add a trailing space if appropriate
|
||
|
if ((numBytes > 1) && (szPos != numBytes - 1))
|
||
|
{
|
||
|
Serial.print(F(" "));
|
||
|
}
|
||
|
}
|
||
|
Serial.println();
|
||
|
}
|
||
|
|
||
|
/**************************************************************************/
|
||
|
/*!
|
||
|
@brief Waits for incoming data and parses it
|
||
|
*/
|
||
|
/**************************************************************************/
|
||
|
uint8_t readPacket(BLEUart *ble_uart, uint16_t timeout)
|
||
|
{
|
||
|
uint16_t origtimeout = timeout, replyidx = 0;
|
||
|
|
||
|
memset(packetbuffer, 0, READ_BUFSIZE);
|
||
|
|
||
|
while (timeout--) {
|
||
|
if (replyidx >= 20) break;
|
||
|
if ((packetbuffer[1] == 'A') && (replyidx == PACKET_ACC_LEN))
|
||
|
break;
|
||
|
if ((packetbuffer[1] == 'G') && (replyidx == PACKET_GYRO_LEN))
|
||
|
break;
|
||
|
if ((packetbuffer[1] == 'M') && (replyidx == PACKET_MAG_LEN))
|
||
|
break;
|
||
|
if ((packetbuffer[1] == 'Q') && (replyidx == PACKET_QUAT_LEN))
|
||
|
break;
|
||
|
if ((packetbuffer[1] == 'B') && (replyidx == PACKET_BUTTON_LEN))
|
||
|
break;
|
||
|
if ((packetbuffer[1] == 'C') && (replyidx == PACKET_COLOR_LEN))
|
||
|
break;
|
||
|
if ((packetbuffer[1] == 'L') && (replyidx == PACKET_LOCATION_LEN))
|
||
|
break;
|
||
|
|
||
|
while (ble_uart->available()) {
|
||
|
char c = ble_uart->read();
|
||
|
if (c == '!') {
|
||
|
replyidx = 0;
|
||
|
}
|
||
|
packetbuffer[replyidx] = c;
|
||
|
replyidx++;
|
||
|
timeout = origtimeout;
|
||
|
}
|
||
|
|
||
|
if (timeout == 0) break;
|
||
|
delay(1);
|
||
|
}
|
||
|
|
||
|
packetbuffer[replyidx] = 0; // null term
|
||
|
|
||
|
if (!replyidx) // no data or timeout
|
||
|
return 0;
|
||
|
if (packetbuffer[0] != '!') // doesn't start with '!' packet beginning
|
||
|
return 0;
|
||
|
|
||
|
// check checksum!
|
||
|
uint8_t xsum = 0;
|
||
|
uint8_t checksum = packetbuffer[replyidx-1];
|
||
|
|
||
|
for (uint8_t i=0; i<replyidx-1; i++) {
|
||
|
xsum += packetbuffer[i];
|
||
|
}
|
||
|
xsum = ~xsum;
|
||
|
|
||
|
// Throw an error message if the checksum's don't match
|
||
|
if (xsum != checksum)
|
||
|
{
|
||
|
Serial.print("Checksum mismatch in packet : ");
|
||
|
printHex(packetbuffer, replyidx+1);
|
||
|
return 0;
|
||
|
}
|
||
|
|
||
|
// checksum passed!
|
||
|
return replyidx;
|
||
|
}
|
||
|
|