90 lines
2.1 KiB
C++
90 lines
2.1 KiB
C++
#include <string.h>
|
|
#include <Arduino.h>
|
|
#include <SPI.h>
|
|
#if not defined (_VARIANT_ARDUINO_DUE_X_) && not defined (_VARIANT_ARDUINO_ZERO_)
|
|
#include <SoftwareSerial.h>
|
|
#endif
|
|
|
|
#include "Adafruit_BLE.h"
|
|
#include "Adafruit_BluefruitLE_SPI.h"
|
|
#include "Adafruit_BluefruitLE_UART.h"
|
|
|
|
#define PACKET_PATTERN_LEN (3)
|
|
#define PACKET_COLOR_LEN (6)
|
|
|
|
// 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];
|
|
|
|
void printHex(const uint8_t * data, const uint32_t numBytes) {
|
|
uint32_t szPos;
|
|
for (szPos=0; szPos < numBytes; szPos++) {
|
|
Serial.print(F("0x"));
|
|
Serial.print(data[szPos], HEX);
|
|
// Add a trailing space if appropriate
|
|
if ((numBytes > 1) && (szPos != numBytes - 1)) {
|
|
Serial.print(F(" "));
|
|
}
|
|
}
|
|
Serial.println();
|
|
}
|
|
|
|
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] == 'P') && (replyidx == PACKET_PATTERN_LEN))
|
|
break;
|
|
if ((packetbuffer[1] == 'C') && (replyidx == PACKET_COLOR_LEN))
|
|
break;
|
|
|
|
while (ble->available()) {
|
|
char c = ble->read();
|
|
if (c == '!') {
|
|
replyidx = 0;
|
|
}
|
|
packetbuffer[replyidx] = c;
|
|
//Serial.print(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;
|
|
}
|
|
|