AP_Compass: allow simulation of compass sensor failure

libraries/AP_Compass/AP_Compass_SITL.cpp

@@ -128,12 +128,22 @@ void AP_Compass_SITL::_timer()
         } else {
             f.rotate(get_board_orientation());
         }
-        if (i == 0) {
-            // scale the first compass to simulate sensor scale factor errors
-            f *= _sitl->mag_scaling;
+        // scale the compass to simulate sensor scale factor errors
+        f *= _sitl->mag_scaling[i];
+
+        switch (_sitl->mag_fail[i]) {
+        case 0:
+            accumulate_sample(f, _compass_instance[i], 10);
+            _last_data[i] = f;
+            break;
+        case 1:
+            // no data
+            break;
+        case 2:
+            // frozen compass
+            accumulate_sample(_last_data[i], _compass_instance[i], 10);
+            break;
         }
-        
-        accumulate_sample(f, _compass_instance[i], 10);
     }
 }
 

libraries/AP_Compass/AP_Compass_SITL.h

@@ -40,5 +40,6 @@ private:
     Matrix3f _eliptical_corr;
     Vector3f _last_dia;
     Vector3f _last_odi;
+    Vector3f _last_data[MAX_SITL_COMPASSES];
 };
 #endif // CONFIG_HAL_BOARD