diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
index 3e37fec74b..d27ee6b009 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@@ -1237,6 +1237,9 @@ void AP_InertialSensor::set_accel(uint8_t instance, const Vector3f &accel)
         if (_accel_count <= instance) {
             _accel_count = instance+1;
         }
+        if (!_accel_healthy[_primary_accel]) {
+            _primary_accel = instance;
+        }
     }
 }
 
@@ -1253,6 +1256,9 @@ void AP_InertialSensor::set_gyro(uint8_t instance, const Vector3f &gyro)
             _gyro_count = instance+1;
             _gyro_cal_ok[instance] = true;
         }
+        if (!_accel_healthy[_primary_accel]) {
+            _primary_accel = instance;
+        }
     }
 }