Merge pull request #5 from InvenSenseInc/features/filter_invalid_imu_data

Filter IMU invalid data:

Author: rbuisson-invn

README.md

@@ -178,6 +178,34 @@ void loop() {
 }
 ```
 
+**bool isAccelDataValid(inv_imu_sensor_event_t *evt)**
+
+This method checks if the accelerometer data in the FIFO sample is valid.
+
+```C++
+void event_cb(inv_imu_sensor_event_t *evt)
+{
+  if(IMU.isAccelDataValid(evt)) {
+    ...
+  }
+}
+
+```
+
+**bool isGyroDataValid(inv_imu_sensor_event_t *evt)**
+
+This method checks if the gyroscope data in the FIFO sample is valid.
+
+```C++
+void event_cb(inv_imu_sensor_event_t *evt)
+{
+  if(IMU.isGyroDataValid(evt)) {
+    ...
+  }
+}
+
+```
+
 **inv_imu_sensor_event_t**
 
 This structure is used by the ICM42670P driver to return raw sensor data. Available data is:

examples/FIFO_Interrupt_SPI/FIFO_Interrupt_SPI.ino

@@ -11,19 +11,21 @@ void irq_handler(void) {
 
 void event_cb(inv_imu_sensor_event_t *evt) {
   // Format data for Serial Plotter
-  Serial.print(evt->accel[0]);
-  Serial.print(",");
-  Serial.print(evt->accel[1]);
-  Serial.print(",");
-  Serial.print(evt->accel[2]);
-  Serial.print(",");
-  Serial.print(evt->gyro[0]);
-  Serial.print(",");
-  Serial.print(evt->gyro[1]);
-  Serial.print(",");
-  Serial.print(evt->gyro[2]);
-  Serial.print(",");
-  Serial.println(evt->temperature);
+  if(IMU.isAccelDataValid(evt)&&IMU.isGyroDataValid(evt)) {
+    Serial.print(evt->accel[0]);
+    Serial.print(",");
+    Serial.print(evt->accel[1]);
+    Serial.print(",");
+    Serial.print(evt->accel[2]);
+    Serial.print(",");
+    Serial.print(evt->gyro[0]);
+    Serial.print(",");
+    Serial.print(evt->gyro[1]);
+    Serial.print(",");
+    Serial.print(evt->gyro[2]);
+    Serial.print(",");
+    Serial.println(evt->temperature);
+  }
 }
 
 void setup() {

examples/Polling_I2C/Polling_I2C.ino

@@ -19,6 +19,8 @@ void setup() {
   IMU.startAccel(100,16);
   // Gyro ODR = 100 Hz and Full Scale Range = 2000 dps
   IMU.startGyro(100,2000);
+  // Wait IMU to start
+  delay(100);
   // Plotter axis header
   Serial.println("AccelX,AccelY,AccelZ,GyroX,GyroY,GyroZ,Temperature");
 }

examples/Polling_SPI/Polling_SPI.ino

@@ -19,6 +19,8 @@ void setup() {
   IMU.startAccel(100,16);
   // Gyro ODR = 100 Hz and Full Scale Range = 2000 dps
   IMU.startGyro(100,2000);
+  // Wait IMU to start
+  delay(100);
   // Plotter axis header
   Serial.println("AccelX,AccelY,AccelZ,GyroX,GyroY,GyroZ,Temperature");
 }

src/ICM42670P.cpp

@@ -162,6 +162,14 @@ int ICM42670P::getDataFromFifo(ICM42670P_sensor_event_cb event_cb) {
   return inv_imu_get_data_from_fifo(&icm_driver);
 }
 
+bool ICM42670P::isAccelDataValid(inv_imu_sensor_event_t *evt) {
+  return (evt->sensor_mask & (1<<INV_SENSOR_ACCEL));
+}
+
+bool ICM42670P::isGyroDataValid(inv_imu_sensor_event_t *evt) {
+  return (evt->sensor_mask & (1<<INV_SENSOR_GYRO));
+}
+
 static int i2c_write(struct inv_imu_serif * serif, uint8_t reg, const uint8_t * wbuffer, uint32_t wlen) {
   i2c->beginTransmission(i2c_address);
   i2c->write(reg);

src/ICM42670P.h

@@ -50,6 +50,8 @@ class ICM42670P {
     int getDataFromRegisters(inv_imu_sensor_event_t* evt);
     int enableFifoInterrupt(uint8_t intpin, ICM42670P_irq_handler handler, uint8_t fifo_watermark);
     int getDataFromFifo(ICM42670P_sensor_event_cb event_cb);
+    bool isAccelDataValid(inv_imu_sensor_event_t *evt);
+    bool isGyroDataValid(inv_imu_sensor_event_t *evt);
 
   protected:
     struct inv_imu_device icm_driver;