From bcabaf0e74c42efcd0d8a38f15bd5c72f3b26b16 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 19 Nov 2022 17:33:16 +0000 Subject: [PATCH] Copter: inform GCS that turtle mode is active --- ArduCopter/mode.h | 1 + ArduCopter/mode_turtle.cpp | 8 +++++++- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 1bd3307bd3..eba43de26d 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -1665,6 +1665,7 @@ private: float motors_output; Vector2f motors_input; + uint32_t last_throttle_warning_output_ms; }; #endif diff --git a/ArduCopter/mode_turtle.cpp b/ArduCopter/mode_turtle.cpp index 2e0aef1c67..99c5d2b08d 100644 --- a/ArduCopter/mode_turtle.cpp +++ b/ArduCopter/mode_turtle.cpp @@ -159,7 +159,7 @@ void ModeTurtle::run() // at this point we have a power value in the range 0..1 - // notmalise the roll and pitch input to match the motors + // normalise the roll and pitch input to match the motors Vector2f input{sign_roll, sign_pitch}; motors_input = input.normalized() * 0.5; // we bypass spin min and friends in the deadzone because we only want spin up when the sticks are moved @@ -171,6 +171,12 @@ void ModeTurtle::output_to_motors() { // throttle needs to be raised if (is_zero(channel_throttle->norm_input_dz())) { + const uint32_t now = AP_HAL::millis(); + if (now - last_throttle_warning_output_ms > 5000) { + gcs().send_text(MAV_SEVERITY_WARNING, "Turtle: raise throttle to arm"); + last_throttle_warning_output_ms = now; + } + disarm_motors(); return; }