1  // SPDX-FileCopyrightText: 2016 James DeVito for Adafruit Industries
  2  //
  3  // SPDX-License-Identifier: MIT
  4  //
  5  #include <string.h>
  6  #include <Arduino.h>
  7  #include <SPI.h>
  8  #include <SoftwareSerial.h>
  9  
 10  #include "Adafruit_BLE.h"
 11  #include "Adafruit_BluefruitLE_SPI.h"
 12  #include "Adafruit_BluefruitLE_UART.h"
 13  
 14  
 15  #define PACKET_ACC_LEN                  (15)
 16  #define PACKET_GYRO_LEN                 (15)
 17  #define PACKET_MAG_LEN                  (15)
 18  #define PACKET_QUAT_LEN                 (19)
 19  #define PACKET_BUTTON_LEN               (5)
 20  #define PACKET_COLOR_LEN                (6)
 21  #define PACKET_LOCATION_LEN             (15)
 22  
 23  //    READ_BUFSIZE            Size of the read buffer for incoming packets
 24  #define READ_BUFSIZE                    (20)
 25  
 26  
 27  /* Buffer to hold incoming characters */
 28  uint8_t packetbuffer[READ_BUFSIZE+1];
 29  
 30  /**************************************************************************/
 31  /*!
 32      @brief  Casts the four bytes at the specified address to a float
 33  */
 34  /**************************************************************************/
 35  float parsefloat(uint8_t *buffer) 
 36  {
 37    float f = ((float *)buffer)[0];
 38    return f;
 39  }
 40  
 41  /**************************************************************************/
 42  /*! 
 43      @brief  Prints a hexadecimal value in plain characters
 44      @param  data      Pointer to the byte data
 45      @param  numBytes  Data length in bytes
 46  */
 47  /**************************************************************************/
 48  void printHex(const uint8_t * data, const uint32_t numBytes)
 49  {
 50    uint32_t szPos;
 51    for (szPos=0; szPos < numBytes; szPos++) 
 52    {
 53      Serial.print(F("0x"));
 54      // Append leading 0 for small values
 55      if (data[szPos] <= 0xF)
 56      {
 57        Serial.print(F("0"));
 58        Serial.print(data[szPos] & 0xf, HEX);
 59      }
 60      else
 61      {
 62        Serial.print(data[szPos] & 0xff, HEX);
 63      }
 64      // Add a trailing space if appropriate
 65      if ((numBytes > 1) && (szPos != numBytes - 1))
 66      {
 67        Serial.print(F(" "));
 68      }
 69    }
 70    Serial.println();
 71  }
 72  
 73  /**************************************************************************/
 74  /*!
 75      @brief  Waits for incoming data and parses it
 76  */
 77  /**************************************************************************/
 78  uint8_t readPacket(Adafruit_BLE *ble, uint16_t timeout) 
 79  {
 80    uint16_t origtimeout = timeout, replyidx = 0;
 81  
 82    memset(packetbuffer, 0, READ_BUFSIZE);
 83  
 84    while (timeout--) {
 85      if (replyidx >= 20) break;
 86      if ((packetbuffer[1] == 'A') && (replyidx == PACKET_ACC_LEN))
 87        break;
 88      if ((packetbuffer[1] == 'G') && (replyidx == PACKET_GYRO_LEN))
 89        break;
 90      if ((packetbuffer[1] == 'M') && (replyidx == PACKET_MAG_LEN))
 91        break;
 92      if ((packetbuffer[1] == 'Q') && (replyidx == PACKET_QUAT_LEN))
 93        break;
 94      if ((packetbuffer[1] == 'B') && (replyidx == PACKET_BUTTON_LEN))
 95        break;
 96      if ((packetbuffer[1] == 'C') && (replyidx == PACKET_COLOR_LEN))
 97        break;
 98      if ((packetbuffer[1] == 'L') && (replyidx == PACKET_LOCATION_LEN))
 99        break;
100  
101      while (ble->available()) {
102        char c =  ble->read();
103        if (c == '!') {
104          replyidx = 0;
105        }
106        packetbuffer[replyidx] = c;
107        replyidx++;
108        timeout = origtimeout;
109      }
110      
111      if (timeout == 0) break;
112      delay(1);
113    }
114  
115    packetbuffer[replyidx] = 0;  // null term
116  
117    if (!replyidx)  // no data or timeout 
118      return 0;
119    if (packetbuffer[0] != '!')  // doesn't start with '!' packet beginning
120      return 0;
121    
122    // check checksum!
123    uint8_t xsum = 0;
124    uint8_t checksum = packetbuffer[replyidx-1];
125    
126    for (uint8_t i=0; i<replyidx-1; i++) {
127      xsum += packetbuffer[i];
128    }
129    xsum = ~xsum;
130  
131    // Throw an error message if the checksum's don't match
132    if (xsum != checksum)
133    {
134      Serial.print("Checksum mismatch in packet : ");
135      printHex(packetbuffer, replyidx+1);
136      return 0;
137    }
138    
139    // checksum passed!
140    return replyidx;
141  }
142