Create test for sensor error loop

ArduCopter/GCS_Mavlink.cpp

@@ -173,6 +173,9 @@ void GCS_MAVLINK_Copter::send_nav_controller_output() const
 
 int16_t GCS_MAVLINK_Copter::vfr_hud_throttle() const
 {
+    if (copter.motors == nullptr) {
+        return 0;
+    }
     return (int16_t)(copter.motors->get_throttle() * 100);
 }
 

Tools/autotest/ardusub.py

@@ -170,6 +170,13 @@ def reboot_sitl(self):
             pass
         self.initialise_after_reboot_sitl()
 
+    def disabled_tests(self):
+        ret = super(AutoTestSub, self).disabled_tests()
+        ret.update({
+            "SensorConfigErrorLoop": "Sub does not instantiate AP_Stats.  Also see https://github.com/ArduPilot/ardupilot/issues/10247",
+        })
+        return ret
+
     def tests(self):
         '''return list of all tests'''
         ret = super(AutoTestSub, self).tests()

Tools/autotest/common.py

@@ -293,7 +293,7 @@ def fetch_parameters(self):
         self.mavproxy.send("param fetch\n")
         self.mavproxy.expect("Received [0-9]+ parameters")
 
-    def reboot_sitl_mav(self):
+    def reboot_sitl_mav(self, required_bootcount=None):
         """Reboot SITL instance using mavlink and wait for it to reconnect."""
         old_bootcount = self.get_parameter('STAT_BOOTCNT')
         # ardupilot SITL may actually NAK the reboot; replace with
@@ -309,26 +309,30 @@ def reboot_sitl_mav(self):
                                        0,
                                        0,
                                        0)
-        self.detect_and_handle_reboot(old_bootcount)
+        self.detect_and_handle_reboot(old_bootcount, required_bootcount=required_bootcount)
 
-    def reboot_sitl(self):
+    def reboot_sitl(self, required_bootcount=None):
         """Reboot SITL instance and wait for it to reconnect."""
-        self.reboot_sitl_mav()
+        self.reboot_sitl_mav(required_bootcount=required_bootcount)
         self.assert_simstate_location_is_at_startup_location()
 
-    def reboot_sitl_mavproxy(self):
+    def reboot_sitl_mavproxy(self, required_bootcount=None):
         """Reboot SITL instance using MAVProxy and wait for it to reconnect."""
         old_bootcount = self.get_parameter('STAT_BOOTCNT')
         self.mavproxy.send("reboot\n")
-        self.detect_and_handle_reboot(old_bootcount)
+        self.detect_and_handle_reboot(old_bootcount, required_bootcount=required_bootcount)
 
-    def detect_and_handle_reboot(self, old_bootcount):
+    def detect_and_handle_reboot(self, old_bootcount, required_bootcount=None):
         tstart = time.time()
+        if required_bootcount is None:
+            required_bootcount = old_bootcount + 1
         while True:
             if time.time() - tstart > 10:
                 raise AutoTestTimeoutException("Did not detect reboot")
             try:
-                if self.get_parameter('STAT_BOOTCNT', timeout=1) != old_bootcount:
+                current_bootcount = self.get_parameter('STAT_BOOTCNT', timeout=1)
+                print("current=%s required=%u" % (str(current_bootcount), required_bootcount))
+                if current_bootcount == required_bootcount:
                     break
             except NotAchievedException:
                 pass
@@ -2599,6 +2603,36 @@ def clear_mission(self):
         if num_wp != 0:
             raise NotAchievedException("Failed to clear mission")
 
+    def test_sensor_config_error_loop(self):
+        '''test the sensor config error loop works and that parameter sets are persistent'''
+        parameter_name = "SERVO8_MIN"
+        old_parameter_value = self.get_parameter(parameter_name)
+        old_sim_baro_count = self.get_parameter("SIM_BARO_COUNT")
+        new_parameter_value = old_parameter_value + 5
+        ex = None
+        try:
+            self.set_parameter("STAT_BOOTCNT", 0)
+            self.set_parameter("SIM_BARO_COUNT", 0)
+            self.reboot_sitl(required_bootcount=1);
+            self.progress("Waiting for 'Check BRD_TYPE'")
+            self.mavproxy.expect("Check BRD_TYPE");
+            self.progress("Setting %s to %f" % (parameter_name, new_parameter_value))
+            self.set_parameter(parameter_name, new_parameter_value)
+        except Exception as e:
+            ex = e
+
+        self.progress("Resetting SIM_BARO_COUNT")
+        self.set_parameter("SIM_BARO_COUNT", old_sim_baro_count)
+
+        self.progress("Calling reboot-sitl ")
+        self.reboot_sitl(required_bootcount=2);
+
+        if ex is not None:
+            raise ex
+
+        if self.get_parameter(parameter_name) != new_parameter_value:
+            raise NotAchievedException("Parameter value did not stick")
+
     def test_gripper(self):
         self.context_push()
         self.set_parameter("GRIP_ENABLE", 1)
@@ -2862,6 +2896,10 @@ def tests(self):
             ("SetHome",
             "Test Set Home",
              self.fly_test_set_home),
+
+            ("SensorConfigErrorLoop",
+             "Test Sensor Config Error Loop",
+             self.test_sensor_config_error_loop),
         ]
 
     def post_tests_announcements(self):

libraries/AP_Baro/AP_Baro.cpp

@@ -452,11 +452,6 @@ void AP_Baro::init(void)
         return;
     }
 
-#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
-    ADD_BACKEND(new AP_Baro_SITL(*this));
-    return;
-#endif
-
 #if HAL_WITH_UAVCAN
     // Detect UAVCAN Modules, try as many times as there are driver slots
     for (uint8_t i = 0; i < BARO_MAX_DRIVERS; i++) {
@@ -542,6 +537,17 @@ void AP_Baro::init(void)
     default:
         break;
     }
+#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    SITL::SITL *sitl = AP::sitl();
+    if (sitl == nullptr) {
+        AP_HAL::panic("No SITL pointer");
+    }
+    if (sitl->baro_count > 1) {
+        ::fprintf(stderr, "More than one baro not supported.  Sorry.");
+    }
+    if (sitl->baro_count == 1) {
+        ADD_BACKEND(new AP_Baro_SITL(*this));
+    }
 #elif HAL_BARO_DEFAULT == HAL_BARO_HIL
     drivers[0] = new AP_Baro_HIL(*this);
     _num_drivers = 1;

libraries/AP_HAL_SITL/HAL_SITL_Class.cpp

@@ -191,6 +191,11 @@ void HAL_SITL::run(int argc, char * const argv[], Callbacks* callbacks) const
         }
     }
 
+    actually_reboot();
+}
+
+void HAL_SITL::actually_reboot()
+{
     execv(new_argv[0], new_argv);
     AP_HAL::panic("PANIC: REBOOT FAILED: %s", strerror(errno));
 }

libraries/AP_HAL_SITL/HAL_SITL_Class.h

@@ -12,6 +12,7 @@ class HAL_SITL : public AP_HAL::HAL {
 public:
     HAL_SITL();
     void run(int argc, char * const argv[], Callbacks* callbacks) const override;
+    static void actually_reboot();
 
 private:
     HALSITL::SITL_State *_sitl_state;

libraries/AP_HAL_SITL/Scheduler.cpp

@@ -5,6 +5,7 @@
 #include "UARTDriver.h"
 #include <sys/time.h>
 #include <fenv.h>
+#include <AP_BoardConfig/AP_BoardConfig.h>
 #if defined (__clang__)
 #include <stdlib.h>
 #else
@@ -137,6 +138,12 @@ void Scheduler::sitl_end_atomic() {
 
 void Scheduler::reboot(bool hold_in_bootloader)
 {
+    if (AP_BoardConfig::in_sensor_config_error()) {
+        // the _should_reboot flag set below is not checked by the
+        // sensor-config-error loop, so force the reboot here:
+        HAL_SITL::actually_reboot();
+        abort();
+    }
     _should_reboot = true;
 }
 

libraries/SITL/SITL.cpp

@@ -166,8 +166,10 @@ const AP_Param::GroupInfo SITL::var_info2[] = {
     AP_GROUPINFO("TWIST_TIME",  40, SITL,  twist.t, 0),
 
     AP_GROUPINFO("GND_BEHAV",   41, SITL,  gnd_behav, -1),
+    AP_GROUPINFO("BARO_COUNT",  42, SITL,  baro_count,  1),
 
     AP_GROUPEND
+
 };
 
 

libraries/SITL/SITL.h

@@ -176,6 +176,7 @@ class SITL {
     AP_Int8  odom_enable; // enable visual odomotry data
     AP_Int8  telem_baudlimit_enable; // enable baudrate limiting on links
     AP_Float flow_noise; // optical flow measurement noise (rad/sec)
+    AP_Int8  baro_count; // number of simulated baros to create
 
     // wind control
     enum WindType {