Check if the IMU is 'good' and periodically re-initialise if not.

roverif/imu.cc

@@ -27,7 +27,7 @@ enum Codes {
 };
 
 IMU::IMU()
-    : mpu_(MPU6050_ADDRESS_AD0_HIGH) {
+    : mpu_(MPU6050_ADDRESS_AD0_HIGH), good_(false) {
 }
 
 void IMU::init() {
@@ -50,6 +50,7 @@ uint8_t IMU::wait() {
 
 bool IMU::read(Protocol::IMU* pinto) {
     if (!mpu_.testConnection()) {
+        good_ = false;
         return false;
     }
 
@@ -59,9 +60,16 @@ bool IMU::read(Protocol::IMU* pinto) {
                     pinto->gyros + 0,
                     pinto->gyros + 1,
                     pinto->gyros + 2);
+    good_ = true;
     return true;
 }
 
+void IMU::check() {
+    if (!good_) {
+        init();
+    }
+}
+
 #define ERRORX(x) { dbg("err " #x "\r\n"); return -x; }
 #define ERROR(x) { return -x; }
 #define DBG(x)

roverif/imu.h

@@ -10,6 +10,7 @@ public:
 
     void init();
     bool read(Protocol::IMU* pinto);
+    void check();
 
 private:
     friend class I2Cdev;
@@ -21,5 +22,6 @@ private:
     static int8_t set_address(uint8_t device, uint8_t address);
 
     MPU6050 mpu_;
+    bool good_;
 };
 

roverif/main.cc

@@ -19,6 +19,7 @@ uint8_t RoverIf::pilot_supplied_;
 uint8_t RoverIf::demands_;
 
 Timer imu_timer;
+Timer check_timer;
 Timer blinker_timer;
 Timer heartbeat_timer;
 Timer pwmin_limiter;
@@ -202,6 +203,9 @@ void RoverIf::tick() {
         defer(Pending::Heartbeat);
         defer(Pending::Counters);
     }
+    if (check_timer.tick(500)) {
+        imu.check();
+    }
 }
 
 #define DISPATCH_REQUEST(_name) \