Copter: Tradheli - disables inverted flight option for single and dual heli frames

This commit is contained in:
bnsgeyer 2018-05-12 22:42:38 -04:00 committed by Andrew Tridgell
parent cc899d74e2
commit 86440b20ba
2 changed files with 21 additions and 11 deletions

View File

@ -221,6 +221,13 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure)
if (!copter.motors->parameter_check(display_failure)) {
return false;
}
// Inverted flight feature disabled for Heli Single and Dual frames
if (copter.g2.frame_class.get() != AP_Motors::MOTOR_FRAME_HELI_QUAD && (copter.g.ch7_option == 43 || copter.g.ch8_option == 43 || copter.g.ch9_option == 43 || copter.g.ch10_option == 43 || copter.g.ch11_option == 43 || copter.g.ch12_option == 43)) {
if (display_failure) {
gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Inverted flight option not supported");
}
return false;
}
#endif // HELI_FRAME
// check for missing terrain data

View File

@ -635,17 +635,20 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
case AUXSW_INVERTED:
#if FRAME_CONFIG == HELI_FRAME
switch (ch_flag) {
case AUX_SWITCH_HIGH:
motors->set_inverted_flight(true);
attitude_control->set_inverted_flight(true);
heli_flags.inverted_flight = true;
break;
case AUX_SWITCH_LOW:
motors->set_inverted_flight(false);
attitude_control->set_inverted_flight(false);
heli_flags.inverted_flight = false;
break;
// inverted flight option is disabled for heli single and dual frames
if (g2.frame_class.get() == AP_Motors::MOTOR_FRAME_HELI_QUAD) {
switch (ch_flag) {
case AUX_SWITCH_HIGH:
motors->set_inverted_flight(true);
attitude_control->set_inverted_flight(true);
heli_flags.inverted_flight = true;
break;
case AUX_SWITCH_LOW:
motors->set_inverted_flight(false);
attitude_control->set_inverted_flight(false);
heli_flags.inverted_flight = false;
break;
}
}
#endif
break;