Use different queues and functions to handle short and long sensor payloads

Author: giulcioffi

src/Arduino_BHY2Host.cpp

@@ -49,8 +49,16 @@ void Arduino_BHY2Host::update()
     } else {
       uint8_t available = availableSensorData();
       for (int i = 0; i < available; i++) {
-        SensorLongDataPacket data;
+        SensorDataPacket data;
         readSensorData(data);
+        SensorLongDataPacket longData;
+        memcpy(&longData, &data, sizeof(SensorDataPacket));
+        sensorManager.process(longData);
+      }
+      uint8_t availableLong = availableSensorLongData();
+      for (int i = 0; i < available; i++) {
+        SensorLongDataPacket data;
+        readSensorLongData(data);
         sensorManager.process(data);
       }
     }
@@ -99,11 +107,21 @@ uint8_t Arduino_BHY2Host::availableSensorData()
   return eslovHandler.requestAvailableData();
 }
 
-bool Arduino_BHY2Host::readSensorData(SensorLongDataPacket &data)
+uint8_t Arduino_BHY2Host::availableSensorLongData()
+{
+  return eslovHandler.requestAvailableLongData();
+}
+
+bool Arduino_BHY2Host::readSensorData(SensorDataPacket &data)
 {
   return eslovHandler.requestSensorData(data);
 }
 
+bool Arduino_BHY2Host::readSensorLongData(SensorLongDataPacket &data)
+{
+  return eslovHandler.requestSensorLongData(data);
+}
+
 void Arduino_BHY2Host::parse(SensorDataPacket& data, DataXYZ& vector)
 {
   DataParser::parse3DVector(data, vector);

src/Arduino_BHY2Host.h

@@ -41,7 +41,9 @@ class Arduino_BHY2Host {
   void configureSensor(uint8_t sensorId, float sampleRate, uint32_t latency);
   uint8_t requestAck();
   uint8_t availableSensorData();
-  bool readSensorData(SensorLongDataPacket &data);
+  uint8_t availableSensorLongData();
+  bool readSensorData(SensorDataPacket &data);
+  bool readSensorLongData(SensorLongDataPacket &data);
 
   void parse(SensorDataPacket& data, DataXYZ& vector);
   void parse(SensorDataPacket& data, DataOrientation& vector);

src/EslovHandler.cpp

@@ -90,16 +90,31 @@ void EslovHandler::update()
       uint8_t availableData = requestAvailableData();
       Serial.write(availableData);
 
-      SensorLongDataPacket sensorData;
+      SensorDataPacket sensorData;
       while (availableData) {
         //delay(ESLOV_DELAY);
         requestSensorData(sensorData);
         delay(ESLOV_DELAY);
-        if (sensorData.sensorId == 115 || sensorData.sensorId == 171) {
-          Serial.write((uint8_t*)&sensorData, sizeof(SensorLongDataPacket));
-        } else {
-          Serial.write((uint8_t*)&sensorData, sizeof(SensorDataPacket));
-        }
+        Serial.write((uint8_t*)&sensorData, sizeof(SensorDataPacket));
+        availableData--;
+      }
+
+      _rxIndex = 0;
+
+    } else if (_rxBuffer[0] == HOST_READ_LONG_SENSOR_OPCODE) {
+
+      if (_debug) {
+        _debug->print("received read sensor opcode\r\n");
+      }
+      uint8_t availableData = requestAvailableLongData();
+      Serial.write(availableData);
+
+      SensorLongDataPacket sensorData;
+      while (availableData) {
+        //delay(ESLOV_DELAY);
+        requestSensorLongData(sensorData);
+        delay(ESLOV_DELAY);
+        Serial.write((uint8_t*)&sensorData, sizeof(SensorLongDataPacket));
         availableData--;
       }
 
@@ -218,23 +233,45 @@ uint8_t EslovHandler::requestAvailableData()
   delay(ESLOV_DELAY);
 }
 
-bool EslovHandler::requestSensorData(SensorLongDataPacket &sData)
+uint8_t EslovHandler::requestAvailableLongData() 
+{
+  writeStateChange(ESLOV_AVAILABLE_LONG_SENSOR_STATE);
+  while(!digitalRead(_eslovIntPin)) {}
+  uint8_t ret = Wire.requestFrom(ESLOV_DEFAULT_ADDRESS, 1);
+  if (!ret) return 0;
+  return Wire.read();
+  delay(ESLOV_DELAY);
+}
+
+bool EslovHandler::requestSensorData(SensorDataPacket &sData)
 {
   if (_eslovState != ESLOV_READ_SENSOR_STATE) {
     writeStateChange(ESLOV_READ_SENSOR_STATE);
     while(!digitalRead(_eslovIntPin)) {}
   }
+  //uint8_t ret = Wire.requestFrom(ESLOV_DEFAULT_ADDRESS, sizeof(SensorDataPacket));
   uint8_t ret = Wire.requestFrom(ESLOV_DEFAULT_ADDRESS, sizeof(SensorDataPacket));
   if (!ret) return false;
 
   uint8_t *data = (uint8_t*)&sData;
   for (uint8_t i = 0; i < sizeof(SensorDataPacket); i++) {
     data[i] = Wire.read();
   }
-  if (data[0] == 115 || data[0] == 171) {
-    for (uint8_t i = sizeof(SensorDataPacket); i < (sizeof(SensorLongDataPacket) - sizeof(SensorDataPacket)); i++) {
-      data[i] = Wire.read();
-    }
+  return true;
+}
+
+bool EslovHandler::requestSensorLongData(SensorLongDataPacket &sData)
+{
+  if (_eslovState != ESLOV_READ_LONG_SENSOR_STATE) {
+    writeStateChange(ESLOV_READ_LONG_SENSOR_STATE);
+    while(!digitalRead(_eslovIntPin)) {}
+  }
+  uint8_t ret = Wire.requestFrom(ESLOV_DEFAULT_ADDRESS, sizeof(SensorLongDataPacket));
+  if (!ret) return false;
+
+  uint8_t *data = (uint8_t*)&sData;
+  for (uint8_t i = 0; i < sizeof(SensorLongDataPacket); i++) {
+    data[i] = Wire.read();
   }
   return true;
 }

src/EslovHandler.h

@@ -24,14 +24,17 @@ enum HostOpcode {
   HOST_DFU_INTERNAL_OPCODE = ESLOV_DFU_INTERNAL_OPCODE,
   HOST_DFU_EXTERNAL_OPCODE = ESLOV_DFU_EXTERNAL_OPCODE,
   HOST_READ_SENSOR_OPCODE,
-  HOST_CONFIG_SENSOR_OPCODE
+  HOST_CONFIG_SENSOR_OPCODE,
+  HOST_READ_LONG_SENSOR_OPCODE
 };
 
 enum EslovState {
   ESLOV_AVAILABLE_SENSOR_STATE = 0x00,
   ESLOV_READ_SENSOR_STATE = 0x01,
   ESLOV_DFU_ACK_STATE = 0x02,
-  ESLOV_SENSOR_ACK_STATE = 0x03
+  ESLOV_SENSOR_ACK_STATE = 0x03,
+  ESLOV_AVAILABLE_LONG_SENSOR_STATE = 0x04,
+  ESLOV_READ_LONG_SENSOR_STATE = 0x05
 
 };
 
@@ -47,8 +50,10 @@ class EslovHandler {
   void writeStateChange(EslovState state);
   void writeConfigPacket(SensorConfigurationPacket& config);
   uint8_t requestPacketAck();
-  uint8_t requestAvailableData() ;
-  bool requestSensorData(SensorLongDataPacket &sData);
+  uint8_t requestAvailableData();
+  uint8_t requestAvailableLongData();
+  bool requestSensorData(SensorDataPacket &sData);
+  bool requestSensorLongData(SensorLongDataPacket &sData);
   void toggleEslovIntPin();
 
 protected: