diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index eed08e4bf8..902baa15b4 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -2512,14 +2512,43 @@ MAV_RESULT GCS_MAVLINK::handle_preflight_reboot(const mavlink_command_long_t &pa { 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); + is_equal(packet.param3, 71.0f)) { + if (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 (is_equal(packet.param4, 94.0f)) { + // the following text is unlikely to make it out... + send_text(MAV_SEVERITY_WARNING,"deferencing a bad thing"); + + void *foo = (void*)0xE000ED38; + + typedef void (*fptr)(); + fptr gptr = (fptr) (void *) foo; + gptr(); + + return MAV_RESULT_FAILED; + } + if (is_equal(packet.param4, 95.0f)) { + // the following text is unlikely to make it out... + send_text(MAV_SEVERITY_WARNING,"calling AP_HAL::panic(...)"); + + AP_HAL::panic("panicing"); + + // keep calm and carry on + } + if (is_equal(packet.param4, 96.0f)) { + // deliberately corrupt parameter storage + send_text(MAV_SEVERITY_WARNING,"wiping parameter storage header"); + StorageAccess param_storage{StorageManager::StorageParam}; + uint8_t zeros[40] {}; + param_storage.write_block(0, zeros, sizeof(zeros)); + return MAV_RESULT_FAILED; } }