@@ -87,11 +87,11 @@ class MPU6050Driver : public Usermod {
8787 int16_t accel_offset[3 ];
8888 };
8989 config_t config;
90+ bool configDirty = true ; // does the configuration need an update?
9091
9192 // MPU control/status vars
9293 bool irqBound = false ; // set true if we have bound the IRQ pin
9394 bool dmpReady = false ; // set true if DMP init was successful
94- uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
9595 uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
9696 uint16_t fifoCount; // count of all bytes currently in FIFO
9797 uint8_t fifoBuffer[64 ]; // FIFO storage buffer
@@ -157,7 +157,10 @@ class MPU6050Driver : public Usermod {
157157 um_data.u_type [8 ] = UMT_UINT32;
158158 }
159159
160+ configDirty = false ; // we have now accepted the current configuration, success or not
161+
160162 if (!config.enabled ) return ;
163+ // TODO: notice if these have changed ??
161164 if (i2c_scl<0 || i2c_sda<0 ) { DEBUG_PRINTLN (F (" MPU6050: I2C is no good." )); return ; }
162165 // Check the interrupt pin
163166 if (config.interruptPin >= 0 ) {
@@ -182,7 +185,7 @@ class MPU6050Driver : public Usermod {
182185
183186 // load and configure the DMP
184187 DEBUG_PRINTLN (F (" Initializing DMP..." ));
185- devStatus = mpu.dmpInitialize ();
188+ auto devStatus = mpu.dmpInitialize ();
186189
187190 // set offsets (from config)
188191 mpu.setXGyroOffset (config.gyro_offset [0 ]);
@@ -241,6 +244,8 @@ class MPU6050Driver : public Usermod {
241244 * loop() is called continuously. Here you can check for events, read sensors, etc.
242245 */
243246 void loop () {
247+ if (configDirty) setup ();
248+
244249 // if programming failed, don't try to do anything
245250 if (!config.enabled || !dmpReady || strip.isUpdating ()) return ;
246251
@@ -407,8 +412,8 @@ class MPU6050Driver : public Usermod {
407412 irqBound = false ;
408413 }
409414
410- // Just re-init
411- setup () ;
415+ // Re-call setup on the next loop()
416+ configDirty = true ;
412417 }
413418
414419 return configComplete;
0 commit comments