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