diff --git a/APMrover2/AP_MotorsUGV.cpp b/APMrover2/AP_MotorsUGV.cpp index 2105a2deb5..3c09daf51f 100644 --- a/APMrover2/AP_MotorsUGV.cpp +++ b/APMrover2/AP_MotorsUGV.cpp @@ -398,6 +398,12 @@ bool AP_MotorsUGV::output_test_pct(motor_test_order motor_seq, float pct) } break; } + case MOTOR_TEST_MAINSAIL: { + if (SRV_Channels::function_assigned(SRV_Channel::k_mainsail_sheet)) { + SRV_Channels::set_output_scaled(SRV_Channel::k_mainsail_sheet, pct); + } + break; + } default: return false; } @@ -452,6 +458,12 @@ bool AP_MotorsUGV::output_test_pwm(motor_test_order motor_seq, float pwm) } break; } + case MOTOR_TEST_MAINSAIL: { + if (SRV_Channels::function_assigned(SRV_Channel::k_mainsail_sheet)) { + SRV_Channels::set_output_pwm(SRV_Channel::k_mainsail_sheet, pwm); + } + break; + } default: return false; } diff --git a/APMrover2/AP_MotorsUGV.h b/APMrover2/AP_MotorsUGV.h index 3dd136aaa2..bf68e63e74 100644 --- a/APMrover2/AP_MotorsUGV.h +++ b/APMrover2/AP_MotorsUGV.h @@ -23,6 +23,7 @@ public: MOTOR_TEST_STEERING = 2, MOTOR_TEST_THROTTLE_LEFT = 3, MOTOR_TEST_THROTTLE_RIGHT = 4, + MOTOR_TEST_MAINSAIL = 5 }; // supported custom motor configurations