mirror of https://github.com/ArduPilot/ardupilot
Copter: inform GCS that turtle mode is active
This commit is contained in:
parent
e02e89a474
commit
bcabaf0e74
|
@ -1665,6 +1665,7 @@ private:
|
|||
|
||||
float motors_output;
|
||||
Vector2f motors_input;
|
||||
uint32_t last_throttle_warning_output_ms;
|
||||
};
|
||||
#endif
|
||||
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue