diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index dfab6165d1..e7e1943b1e 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -578,6 +578,11 @@ protected: void handle_set_gps_global_origin(const mavlink_message_t &msg); void handle_setup_signing(const mavlink_message_t &msg) const; virtual MAV_RESULT handle_preflight_reboot(const mavlink_command_long_t &packet, const mavlink_message_t &msg); + struct { + HAL_Semaphore sem; + bool taken; + } _deadlock_sem; + void deadlock_sem(void); // reset a message interval via mavlink: MAV_RESULT handle_command_set_message_interval(const mavlink_command_long_t &packet); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 781d8e5a0d..a46dcaed10 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -3162,6 +3162,16 @@ MAV_RESULT GCS_MAVLINK::handle_preflight_reboot(const mavlink_command_long_t &pa #endif } #endif + if (is_equal(packet.param4, 100.0f)) { + send_text(MAV_SEVERITY_WARNING,"Creating mutex deadlock"); + hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&GCS_MAVLINK::deadlock_sem, void)); + while (!_deadlock_sem.taken) { + hal.scheduler->delay(1); + } + WITH_SEMAPHORE(_deadlock_sem.sem); + send_text(MAV_SEVERITY_WARNING,"deadlock passed"); + return MAV_RESULT_ACCEPTED; + } } // refuse reboot when armed: @@ -3205,6 +3215,17 @@ MAV_RESULT GCS_MAVLINK::handle_preflight_reboot(const mavlink_command_long_t &pa return MAV_RESULT_FAILED; } +/* + take a semaphore and do not release it, triggering a deadlock + */ +void GCS_MAVLINK::deadlock_sem(void) +{ + if (!_deadlock_sem.taken) { + _deadlock_sem.taken = true; + _deadlock_sem.sem.take_blocking(); + } +} + /* handle a flight termination request */