Merge pull request #32 from arduino/bsec+examples

giulcioffi · web-flow · commit ec4ddae9c79e · 2021-10-13T11:36:52.000+02:00
Add support for Bsec sensors
diff --git a/Arduino_BHY2/src/Arduino_BHY2.h b/Arduino_BHY2/src/Arduino_BHY2.h
@@ -9,6 +9,7 @@
 #include "sensors/SensorOrientation.h"
 #include "sensors/SensorXYZ.h"
 #include "sensors/SensorQuaternion.h"
+#include "sensors/SensorBSEC.h"
 #include "sensors/SensorActivity.h"
 #include "sensors/Sensor.h"
 
diff --git a/Arduino_BHY2/src/sensors/DataParser.cpp b/Arduino_BHY2/src/sensors/DataParser.cpp
@@ -24,6 +24,35 @@ void DataParser::parseQuaternion(SensorDataPacket& data, DataQuaternion& vector)
   vector.accuracy = data.getUint16(8);
 }
 
+void DataParser::parseBSEC(SensorDataPacket& data, DataBSEC& vector) {
+  const float SCALE_BSEC_BVOC_EQ = 0.01f;
+  const float SCALE_BSEC_COMP_T = 1.0f / 256;
+  const float SCALE_BSEC_COMP_H = 1.0f / 500;
+
+  vector.iaq = data.getUint16(0);
+  vector.iaq_s = data.getUint16(2);
+  vector.b_voc_eq = data.getUint16(4) * SCALE_BSEC_BVOC_EQ;  //b-VOC-eq in the FIFO frame is scaled up by 100
+  vector.co2_eq = data.getUint24(6);
+  vector.accuracy = data.getUint8(9);
+  vector.comp_t = data.getInt16(10) * SCALE_BSEC_COMP_T;
+  vector.comp_h = data.getUint16(12) * SCALE_BSEC_COMP_H;
+  vector.comp_g = (uint32_t)(data.getFloat(14));
+}
+
+void DataParser::parseBSECLegacy(SensorDataPacket& data, DataBSEC& vector) {
+  vector.comp_t = data.getFloat(0);
+  vector.comp_h = data.getFloat(4);
+  //note that: SENSOR_DATA_FIXED_LENGTH is defined as 10 by default,
+  //so all the fields below are 0 unless it's redefined to 29 and above
+  vector.comp_g = (uint32_t)(data.getFloat(8));
+  vector.iaq = (uint16_t)(data.getFloat(12));
+  vector.iaq_s = (uint16_t)(data.getFloat(16));
+  vector.co2_eq = (uint32_t)data.getFloat(20);
+  vector.b_voc_eq = data.getFloat(24);
+  vector.accuracy = data.getUint8(28);
+}
+
+
 void DataParser::parseData(SensorDataPacket& data, float& value, float scaleFactor, SensorPayload format) {
   uint8_t id = data.sensorId;
   switch (format) {
@@ -58,4 +87,4 @@ void DataParser::parseData(SensorDataPacket& data, float& value, float scaleFact
 
 void DataParser::parseActivity(SensorDataPacket& data, uint16_t& value) {
   value = data.getUint16(0);
-}
+}
diff --git a/Arduino_BHY2/src/sensors/DataParser.h b/Arduino_BHY2/src/sensors/DataParser.h
@@ -23,8 +23,8 @@ struct DataOrientation {
   float roll;
 
   String toString() {
-    return (String)("Orientation values - heading: " + String(heading, 3) 
-                    + "   pitch: " + String(pitch, 3) 
+    return (String)("Orientation values - heading: " + String(heading, 3)
+                    + "   pitch: " + String(pitch, 3)
                     + "   roll: " + String(roll, 3) + "\n");
   }
 };
@@ -39,19 +39,45 @@ struct DataQuaternion {
   String toString() {
     return (String)("Quaternion values - X: " + String(x)
                     + "   Y: " + String(y)
-                    + "   Z: " + String(z) 
-                    + "   W: " + String(w) 
-                    + "   Accuracy: " + String(accuracy) 
+                    + "   Z: " + String(z)
+                    + "   W: " + String(w)
+                    + "   Accuracy: " + String(accuracy)
                     + "\n");
   }
 };
 
+struct DataBSEC {
+  uint16_t  iaq;         //iaq value for regular use case
+  uint16_t  iaq_s;       //iaq value for stationary use cases
+  float     b_voc_eq;    //breath VOC equivalent (ppm)
+  uint32_t  co2_eq;      //CO2 equivalent (ppm) [400,]
+  float     comp_t;      //compensated temperature (celcius)
+  float     comp_h;      //compensated humidity
+  uint32_t  comp_g;      //compensated gas resistance (Ohms)
+  uint8_t   accuracy;    //accuracy level: [0-3]
+
+  String toString() {
+    return (String)("BSEC output values - iaq: " + String(iaq)
+                    + "   iaq_s: " + String(iaq_s)
+                    + "   b_voc_eq: " + String(b_voc_eq, 2)
+                    + "   co2_eq: " + String(co2_eq)
+                    + "   accuracy: " + String(accuracy)
+                    + "   comp_t: " + String(comp_t, 2)
+                    + "   comp_h: " + String(comp_h, 2)
+                    + "   comp_g: " + String(comp_g)
+                    + "\n");
+  }
+};
+
+
 class DataParser {
 public:
   static void parse3DVector(SensorDataPacket& data, DataXYZ& vector);
   static void parseEuler(SensorDataPacket& data, DataOrientation& vector);
   static void parseEuler(SensorDataPacket& data, DataOrientation& vector, float scaleFactor);
   static void parseQuaternion(SensorDataPacket& data, DataQuaternion& vector);
+  static void parseBSEC(SensorDataPacket& data, DataBSEC& vector);
+  static void parseBSECLegacy(SensorDataPacket& data, DataBSEC& vector);
   static void parseData(SensorDataPacket& data, float& value, float scaleFactor, SensorPayload format);
   static void parseActivity(SensorDataPacket& data, uint16_t& value);
 };
diff --git a/Arduino_BHY2/src/sensors/SensorBSEC.h b/Arduino_BHY2/src/sensors/SensorBSEC.h
@@ -0,0 +1,48 @@
+#ifndef SENSOR_BSEC_H_
+#define SENSOR_BSEC_H_
+
+#include "SensorClass.h"
+
+
+class SensorBSEC : public SensorClass {
+public:
+  SensorBSEC() {}
+  SensorBSEC(uint8_t id) : SensorClass(id), _data() {}
+
+/*
+  BSEC sensor frames are:
+    - 18 bytes for BSEC new format (SID=115 SENSOR_ID_BSEC_LEGACY)
+    - 29 bytes for legacy format (SID=171 SENSOR_ID_BSEC)
+  If the default size of SENSOR_DATA_FIXED_LENGTH is used (10 bytes), some fields of BSEC might be always 0.
+  Enlarge SENSOR_DATA_FIXED_LENGTH to see all the fields of the BSEC sensor.
+  For the new format (SID=115), if the compensated values (comp_t, comp_h, comp_g) are not important,
+  keep SENSOR_DATA_FIXED_LENGTH to the default value (10) to save bandwidth.
+*/
+  uint16_t iaq() {return _data.iaq;}
+  uint16_t iaq_s() {return _data.iaq_s;}
+  float b_voc_eq() {return _data.b_voc_eq;}
+  uint32_t co2_eq() {return _data.co2_eq;}
+  uint8_t accuracy() {return _data.accuracy;}
+  float comp_t() {return _data.comp_t;}
+  float comp_h() {return _data.comp_h;}
+  uint32_t comp_g() {return _data.comp_g;}
+
+
+  void setData(SensorDataPacket &data)
+  {
+      if (_id == SENSOR_ID_BSEC ) {
+          DataParser::parseBSEC(data, _data);
+      } else if (_id == SENSOR_ID_BSEC_LEGACY) {
+          DataParser::parseBSECLegacy(data, _data);
+      }
+  }
+
+  String toString()
+  {
+    return _data.toString();
+  }
+
+private:
+  DataBSEC _data;
+};
+#endif
diff --git a/Arduino_BHY2/src/sensors/SensorID.h b/Arduino_BHY2/src/sensors/SensorID.h
@@ -52,6 +52,7 @@ enum SensorID {
   SENSOR_ID_GYRO_BIAS_WU             = 92,  /* Gyroscope offset wake up */
   SENSOR_ID_MAG_BIAS_WU              = 93,  /* Magnetometer offset wake up */
   SENSOR_ID_STD_WU                   = 94,  /* Step detector wake up */
+  SENSOR_ID_BSEC                     = 115,  /* BSEC 1.x output */
   SENSOR_ID_TEMP                     = 128, /* Temperature */
   SENSOR_ID_BARO                     = 129, /* Barometer */
   SENSOR_ID_HUM                      = 130, /* Humidity */
@@ -74,6 +75,7 @@ enum SensorID {
   SENSOR_ID_PROX                     = 147, /* Proximity */
   SENSOR_ID_LIGHT_WU                 = 148, /* Light wake up */
   SENSOR_ID_PROX_WU                  = 149, /* Proximity wake up */
+  SENSOR_ID_BSEC_LEGACY              = 171, /* BSEC 1.x output (legacy, deprecated) */
   DEBUG_DATA_EVENT                   = 250, /* Binary or string debug data */
   TIMESTAMP_SMALL_DELTA              = 251, /* Incremental time change from previous read */
   TIMESTAMP_SMALL_DELTA_WU           = 245, /* Incremental time change from previous read wake up */
diff --git a/Arduino_BHY2/src/sensors/SensorTypes.h b/Arduino_BHY2/src/sensors/SensorTypes.h
@@ -25,9 +25,11 @@ struct __attribute__((packed)) SensorDataPacket {
     float result = 0;
     uint8_t length = sizeof(result);
     if (index + length > SENSOR_DATA_FIXED_LENGTH) {
-      length = SENSOR_DATA_FIXED_LENGTH - index;
+      //to safe guard against overflow
+      length = SENSOR_DATA_FIXED_LENGTH > index ? SENSOR_DATA_FIXED_LENGTH - index : 0;
     }
-    memcpy(&result, &data[index], sizeof(result));
+    if (length > 0)
+        memcpy(&result, &data[index], length);
     return result;
   }
 
@@ -42,29 +44,32 @@ struct __attribute__((packed)) SensorDataPacket {
     uint16_t result = 0;
     uint8_t length = sizeof(result);
     if (index + length > SENSOR_DATA_FIXED_LENGTH) {
-      length = SENSOR_DATA_FIXED_LENGTH - index;
+      length = SENSOR_DATA_FIXED_LENGTH > index ? SENSOR_DATA_FIXED_LENGTH - index : 0;
     }
-    memcpy(&result, &data[index], length);
+    if (length > 0)
+        memcpy(&result, &data[index], length);
     return result;
   }
 
   uint32_t getUint24(uint8_t index) {
     uint32_t result = 0;
     uint8_t length = 3;
     if (index + length > SENSOR_DATA_FIXED_LENGTH) {
-      length = SENSOR_DATA_FIXED_LENGTH - index;
+      length = SENSOR_DATA_FIXED_LENGTH > index ? SENSOR_DATA_FIXED_LENGTH - index : 0;
     }
-    memcpy(&result, &data[index], length);
+    if (length > 0)
+        memcpy(&result, &data[index], length);
     return result;
   }
 
   uint32_t getUint32(uint8_t index) {
     uint32_t result = 0;
     uint8_t length = sizeof(result);
     if (index + length > SENSOR_DATA_FIXED_LENGTH) {
-      length = SENSOR_DATA_FIXED_LENGTH - index;
+      length = SENSOR_DATA_FIXED_LENGTH > index ? SENSOR_DATA_FIXED_LENGTH - index : 0;
     }
-    memcpy(&result, &data[index], length);
+    if (length > 0)
+        memcpy(&result, &data[index], length);
     return result;
   }
 
@@ -79,19 +84,21 @@ struct __attribute__((packed)) SensorDataPacket {
     int16_t result = 0;
     uint8_t length = sizeof(result);
     if (index + length > SENSOR_DATA_FIXED_LENGTH) {
-      length = SENSOR_DATA_FIXED_LENGTH - index;
+      length = SENSOR_DATA_FIXED_LENGTH > index ? SENSOR_DATA_FIXED_LENGTH - index : 0;
     }
-    memcpy(&result, &data[index], length);
+    if (length > 0)
+        memcpy(&result, &data[index], length);
     return result;
   }
 
   int32_t getInt32(uint8_t index) {
     int32_t result = 0;
     uint8_t length = sizeof(result);
     if (index + length > SENSOR_DATA_FIXED_LENGTH) {
-      length = SENSOR_DATA_FIXED_LENGTH - index;
+      length = SENSOR_DATA_FIXED_LENGTH > index ? SENSOR_DATA_FIXED_LENGTH - index : 0;
     }
-    memcpy(&result, &data[index], length);
+    if (length > 0)
+        memcpy(&result, &data[index], length);
     return result;
   }
 };
diff --git a/tools/bhy-controller/src/sensor/parser.go b/tools/bhy-controller/src/sensor/parser.go
@@ -89,6 +89,10 @@ func parseData(data *SensorData) {
 		typeScheme = typeMap[4].(map[string]interface{})
   } else if (sensorScheme == "BSECOutput") {
 		typeScheme = typeMap[5].(map[string]interface{})
+  } else if (sensorScheme == "BSECOutputV2") {
+		typeScheme = typeMap[6].(map[string]interface{})
+  } else if (sensorScheme == "BSECOutputV2Full") {
+		typeScheme = typeMap[6].(map[string]interface{})
   }
 
 	fields := typeScheme["parse-scheme"].([]interface{})
diff --git a/tools/bhy-controller/src/webserver/parse-scheme.json b/tools/bhy-controller/src/webserver/parse-scheme.json
@@ -63,6 +63,36 @@
         {"name": "Temperature (compensated)", "type": "float", "scale-factor": 1},
         {"name": "Humidity (compensated)", "type": "float", "scale-factor": 1}
       ]
+    },
+
+    {
+      "id": 6,
+      "type": "BSECOutputV2",
+      "parse-scheme":
+      [
+        {"name": "IAQ(Mobile)", "type": "uint16", "scale-factor": 1},
+        {"name": "IAQ(Stationary)", "type": "uint16", "scale-factor": 1},
+        {"name": "bVOC-Equivalents(ppm)", "type": "uint16", "scale-factor": 0.01},
+        {"name": "CO2-Equivalents(ppm)", "type": "uint24", "scale-factor": 1},
+        {"name": "Accuracy", "type": "uint8", "scale-factor": 1}
+      ]
+    },
+
+    {
+      "id": 7,
+      "type": "BSECOutputV2Full",
+      "parse-scheme":
+      [
+        {"name": "IAQ(Mobile)", "type": "uint16", "scale-factor": 1},
+        {"name": "IAQ(Stationary)", "type": "uint16", "scale-factor": 1},
+        {"name": "b-VOC-Equivalents(ppm)", "type": "uint16", "scale-factor": 0.01},
+        {"name": "CO2-Equivalents(ppm)", "type": "uint24", "scale-factor": 1},
+        {"name": "Accuracy", "type": "uint8", "scale-factor": 1},
+        {"name": "Compensated-Temperature(°C)", "type": "int16", "scale-factor": 0.003906},
+        {"name": "Compensated-Humidity(%)", "type": "uint16", "scale-factor": 0.002},
+        {"name": "Compensated-Gas Resistance(Ohms)", "type": "float", "scale-factor": 1}
+      ]
     }
+
   ]
 }
diff --git a/tools/bhy-controller/src/webserver/sensor-type-map.json b/tools/bhy-controller/src/webserver/sensor-type-map.json
@@ -347,6 +347,13 @@
     "dashboard": 0
   },
 
+  "115": {
+    "name": "BSEC Output",
+    "scheme": "BSECOutputV2",
+    "value": 0,
+    "dashboard": 1
+  },
+
   "128": {
     "name": "TEMPERATURE",
     "scheme": "singleRead",
@@ -576,7 +583,7 @@
   },
 
   "171": {
-    "name": "BSEC Output",
+    "name": "BSEC Output (deprecated)",
     "scheme": "BSECOutput",
     "value": 0,
     "dashboard": 0
diff --git a/tools/bhy-controller/src/webserver/sensor.html b/tools/bhy-controller/src/webserver/sensor.html
@@ -429,6 +429,44 @@ <h3>Read sensor data</h3>
           sensorTypes[sensor].value = cnt + 1;
         }
 
+        if (cnt > 150) {
+          Plotly.relayout(chartIdx,{
+            xaxis: {
+              range: [cnt-150,cnt]
+            }
+          });
+        }
+    } else if (sensorTypes[sensor].scheme == "BSECOutputV2") {
+        //Parse BSEC values
+        const BSECValues = parsedStringValue.split(" ");
+        console.log("Split: ", BSECValues);
+
+        var val1 = BSECValues[1];   //iaq
+        var val2 = BSECValues[5];   //iaq-s
+        var val3 = BSECValues[9] * 1000;   //b-voc-eq
+        var val4 = BSECValues[13];  //co2-eq
+        var val5 = BSECValues[17];  //status
+
+        if (sensorTypes[sensor].value == 0) {   //Plot doesn't exist yet because no valid data have been received
+          if (val4 != 0) {   //First valid data received: draw it in the chart, valid CO2-eq output is always greater than 0
+              Plotly.newPlot(chartIdx,
+                  [
+                      {y:[val1],name:'IAQ',type:'line'}, {y:[val2],name:'IAQ-S',type:'line'},
+                      {y:[val3],name:'bVOC-eq (ppb)',type:'line'}, {y:[val4],name:'CO2-eq (ppm)',type:'line'}
+                  ]);
+            //Update json to signal that the reception started
+            cnt = sensorTypes[sensor].value + 1;
+            sensorTypes[sensor].value = cnt;
+          }
+        } else {    //Plot already exists
+          Plotly.extendTraces(chartIdx,{y:[[val1]]}, [0]);
+          Plotly.extendTraces(chartIdx,{y:[[val2]]}, [1]);
+          Plotly.extendTraces(chartIdx,{y:[[val3]]}, [2]);
+          Plotly.extendTraces(chartIdx,{y:[[val4]]}, [3]);
+          cnt = sensorTypes[sensor].value;
+          sensorTypes[sensor].value = cnt + 1;
+        }
+
         if (cnt > 150) {
           Plotly.relayout(chartIdx,{
             xaxis: {
@@ -467,6 +505,13 @@ <h3>Read sensor data</h3>
     parse_scheme = parseScheme["types"][4]["parse-scheme"];
   } else if (scheme == "BSECOutput") {
     parse_scheme = parseScheme["types"][5]["parse-scheme"];
+  } else if (scheme == "BSECOutputV2") {
+      var size = data.getUint8(1);
+      if (size <= 10) {
+         parse_scheme = parseScheme["types"][6]["parse-scheme"];
+      } else {
+         parse_scheme = parseScheme["types"][7]["parse-scheme"];
+      }
   }
 
   parse_scheme.forEach(element => {
