Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: mode_turtle: check motors armed and interlocked

Browse Source
This commit is contained in:
Iampete1 2022-01-27 15:36:21 +00:00 committed by Randy Mackay
parent 4014ec055f
commit b235116262
1 changed files with 4 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduCopter/mode_turtle.cpp
View File

@ -142,6 +142,9 @@ void ModeTurtle::run()
// actually write values to the motors // actually write values to the motors
void ModeTurtle::output_to_motors() void ModeTurtle::output_to_motors()
{ {
// check if motor are allowed to spin
const bool allow_output = motors->armed() && motors->get_interlock();
for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; ++i) { for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; ++i) {
if (!motors->is_motor_enabled(i)) { if (!motors->is_motor_enabled(i)) {
continue; continue;
@ -149,7 +152,7 @@ void ModeTurtle::output_to_motors()
const Vector2f output{motors->get_roll_factor(i), motors->get_pitch_factor(i)}; const Vector2f output{motors->get_roll_factor(i), motors->get_pitch_factor(i)};
// if output aligns with input then use this motor // if output aligns with input then use this motor
if ((motors_input - output).length() > 0.5) { if (!allow_output || (motors_input - output).length() > 0.5) {
motors->rc_write(i, motors->get_pwm_output_min()); motors->rc_write(i, motors->get_pwm_output_min());
continue; continue;
} }