adafruit-mirror/Adafruit_Learning_System_Guides
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

Create packetParser.cpp

Browse source
This commit is contained in:
Mike Barela 2019-05-01 17:27:23 -04:00 committed by GitHub
parent d1741d6925
commit 1347f23711
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
1 changed files with 137 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

137
BLE_Robot_Rover/Ada_BLE_RC/packetParser.cpp Normal file
View file

@ -0,0 +1,137 @@
#include <string.h>
#include <Arduino.h>
#include <SPI.h>
#include <SoftwareSerial.h>
#include "Adafruit_BLE.h"
#include "Adafruit_BluefruitLE_SPI.h"
#include "Adafruit_BluefruitLE_UART.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 = ((float *)buffer)[0];
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(Adafruit_BLE *ble, 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->available()) {
char c = ble->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;
}