From 7f9d0abc06377742bff075ab3f6fb8b78b103772 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 8 Jul 2019 18:02:18 +1000 Subject: [PATCH 1/5] AP_HAL_SITL: fix rebooting while in sensor config error loop --- libraries/AP_HAL_SITL/HAL_SITL_Class.cpp | 5 +++++ libraries/AP_HAL_SITL/HAL_SITL_Class.h | 1 + libraries/AP_HAL_SITL/Scheduler.cpp | 7 +++++++ 3 files changed, 13 insertions(+) diff --git a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp b/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp index a33b42dc2ab1e..51bc6e0931d09 100644 --- a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp +++ b/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)); } diff --git a/libraries/AP_HAL_SITL/HAL_SITL_Class.h b/libraries/AP_HAL_SITL/HAL_SITL_Class.h index 268ee6f424b35..c8bbb1d1cd3af 100644 --- a/libraries/AP_HAL_SITL/HAL_SITL_Class.h +++ b/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; diff --git a/libraries/AP_HAL_SITL/Scheduler.cpp b/libraries/AP_HAL_SITL/Scheduler.cpp index 20d1f1fc0dae0..a74bb93b1378a 100644 --- a/libraries/AP_HAL_SITL/Scheduler.cpp +++ b/libraries/AP_HAL_SITL/Scheduler.cpp @@ -5,6 +5,7 @@ #include "UARTDriver.h" #include #include +#include #if defined (__clang__) #include #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; } From 3c2b701e10f6830559d082b7defe8a4aec2a04c4 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 5 Jan 2019 21:44:40 +1100 Subject: [PATCH 2/5] AP_Baro: support 0 detected simulated baros --- libraries/AP_Baro/AP_Baro.cpp | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index aaf790b08ace3..bf6b9c7433859 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/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; From 9342ba4238b77acc423f0e6de257969f9182125a Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 5 Jan 2019 21:45:02 +1100 Subject: [PATCH 3/5] SITL: support a count of baros --- libraries/SITL/SITL.cpp | 2 ++ libraries/SITL/SITL.h | 1 + 2 files changed, 3 insertions(+) diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index 0da3770548eec..61013453c689e 100644 --- a/libraries/SITL/SITL.cpp +++ b/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 + }; diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index da914abf1ae1a..eb233940da8a8 100644 --- a/libraries/SITL/SITL.h +++ b/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 { From 79001786e5fc89cd661c253ffa207e00fc97e0a8 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 8 Jul 2019 18:24:46 +1000 Subject: [PATCH 4/5] Copter: correct nullptr dereference in sensor-config error loop --- ArduCopter/GCS_Mavlink.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 62102804e24c2..91834919d9f59 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/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); } From 3296d775b941770054f1e6e4849c51ac3cba76ed Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 5 Jan 2019 22:46:45 +1100 Subject: [PATCH 5/5] Tools: autotest: add test for sensor config error loop Tools: autotest: exempt failing vehicles from SensorConfigError test --- Tools/autotest/ardusub.py | 7 +++++ Tools/autotest/common.py | 54 +++++++++++++++++++++++++++++++++------ 2 files changed, 53 insertions(+), 8 deletions(-) diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py index e83eb992a9b20..7d8dc598eff5c 100644 --- a/Tools/autotest/ardusub.py +++ b/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() diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 672d288aaa76b..14c3b03767c98 100644 --- a/Tools/autotest/common.py +++ b/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):