From 2313970eaf3989b6e5a21e51fff24e708edc1f58 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 20 Apr 2019 14:33:35 +1000 Subject: [PATCH] GCS_MAVLink: added method to lockup autopilot used for watchdog testing --- libraries/GCS_MAVLink/GCS_Common.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 36b3d6291c..e2df68ea27 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -2537,6 +2537,19 @@ void GCS_MAVLINK::zero_rc_outputs() */ MAV_RESULT GCS_MAVLINK::handle_preflight_reboot(const mavlink_command_long_t &packet) { + if (is_equal(packet.param1, 42.0f) && + is_equal(packet.param2, 24.0f) && + is_equal(packet.param3, 71.0f) && + is_equal(packet.param4, 93.0f)) { + // this is a magic sequence to force the main loop to + // lockup. This is for testing the stm32 watchdog + // functionality + while (true) { + send_text(MAV_SEVERITY_WARNING,"entering lockup"); + hal.scheduler->delay(250); + } + } + if (hal.util->get_soft_armed()) { // refuse reboot when armed return MAV_RESULT_FAILED;