Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: add turtle mode safety features.

This commit is contained in:
Andy Piper 2022-10-20 19:34:13 +01:00 committed by Andrew Tridgell
parent 24537b2b84
commit f83b8a05fa
2 changed files with 42 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduCopter/mode.h
View File

@ -1641,6 +1641,9 @@ protected:
const char *name4() const override { return "TRTL"; }
private:
void arm_motors();
void disarm_motors();
float motors_output;
Vector2f motors_input;
};

43
ArduCopter/mode_turtle.cpp
View File

@ -18,12 +18,29 @@ bool ModeTurtle::init(bool ignore_checks)
return false;
}
// do not enter the mode if sticks are not centered
// do not enter the mode if sticks are not centered or throttle is not at zero
if (!is_zero(channel_pitch->norm_input_dz())
|| !is_zero(channel_roll->norm_input_dz())
|| !is_zero(channel_yaw->norm_input_dz())) {
|| !is_zero(channel_yaw->norm_input_dz())
|| !is_zero(channel_throttle->norm_input_dz())) {
return false;
}
// turn on notify leds
AP_Notify::flags.esc_calibration = true;
return true;
}
void ModeTurtle::arm_motors()
{
if (hal.util->get_soft_armed()) {
return;
}
// stop the spoolup block activating
motors->set_spoolup_block(false);
// reverse the motors
hal.rcout->disable_channel_mask_updates();
change_motor_direction(true);
@ -36,8 +53,6 @@ bool ModeTurtle::init(bool ignore_checks)
// arm
motors->armed(true);
hal.util->set_soft_armed(true);
return true;
}
bool ModeTurtle::allows_arming(AP_Arming::Method method) const
@ -47,6 +62,18 @@ bool ModeTurtle::allows_arming(AP_Arming::Method method) const
void ModeTurtle::exit()
{
disarm_motors();
// turn off notify leds
AP_Notify::flags.esc_calibration = false;
}
void ModeTurtle::disarm_motors()
{
if (!hal.util->get_soft_armed()) {
return;
}
// disarm
motors->armed(false);
hal.util->set_soft_armed(false);
@ -142,6 +169,14 @@ void ModeTurtle::run()
// actually write values to the motors
void ModeTurtle::output_to_motors()
{
// throttle needs to be raised
if (is_zero(channel_throttle->norm_input_dz())) {
disarm_motors();
return;
}
arm_motors();
// check if motor are allowed to spin
const bool allow_output = motors->armed() && motors->get_interlock();