fixed #258

* fixed #258 * removed debug
rosflight · Sep 29, 2017 · 0a7809c · 0a7809c
1 parent da52758
commit 0a7809c
Show file tree

Hide file tree

Showing 3 changed files with 6 additions and 7 deletions.
diff --git a/boards/naze/naze32.cpp b/boards/naze/naze32.cpp
@@ -106,7 +106,6 @@ uint8_t Naze32::serial_read(void)
 
 void Naze32::sensors_init()
 {
-  _board_revision = 2;
   // Initialize I2c
   i2cInit(I2CDEV_2);
 
@@ -159,8 +158,7 @@ bool Naze32::imu_read(float accel[3], float* temperature, float gyro[3], uint64_
 
 void Naze32::imu_not_responding_error(void)
 {
-  // If the IMU is not responding, then we need to change where we look for the
-  // interrupt
+  // If the IMU is not responding, then we need to change where we look for the interrupt
   _board_revision = (_board_revision < 4) ? 5 : 2;
   sensors_init();
 }

diff --git a/src/rosflight.cpp b/src/rosflight.cpp
@@ -110,7 +110,7 @@ void ROSflight::run()
   /*********************/
   /***  Post-Process ***/
   /*********************/
-//  // internal timers figure out what and when to send
+  // internal timers figure out what and when to send
   mavlink_.stream();
 
   // receive mavlink messages

diff --git a/src/sensors.cpp b/src/sensors.cpp
@@ -281,12 +281,13 @@ bool Sensors::update_imu(void)
   }
   else
   {
-    // if we have lost 10 IMU messages then something is wrong
-    if (rf_.board_.clock_millis() > last_imu_update_ms_ + 10)
+    // if we have lost 1000 IMU messages then something is wrong
+    if (rf_.board_.clock_millis() > last_imu_update_ms_ + 1000)
     {
       // Tell the board to fix it
       last_imu_update_ms_ = rf_.board_.clock_millis();
-      rf_.board_.imu_not_responding_error();
+      if (!rf_.state_manager_.state().armed)
+        rf_.board_.imu_not_responding_error();
 
       // Indicate an IMU error
       rf_.state_manager_.set_error(StateManager::ERROR_IMU_NOT_RESPONDING);