Copter: support inverted flight CH7 option

This commit is contained in:
Andrew Tridgell 2017-08-28 09:51:49 +10:00
parent f07aac396c
commit 951ed95eb9
3 changed files with 20 additions and 0 deletions

View File

@ -631,6 +631,7 @@ private:
struct {
uint8_t dynamic_flight : 1; // 0 // true if we are moving at a significant speed (used to turn on/off leaky I terms)
uint8_t init_targets_on_arming : 1; // 1 // true if we have been disarmed, and need to reset rate controller targets when we arm
uint8_t inverted_flight : 1; // 2 // true for inverted flight mode
} heli_flags;
int16_t hover_roll_trim_scalar_slew;

View File

@ -71,6 +71,7 @@ enum aux_sw_func {
AUXSW_AVOID_PROXIMITY = 40, // enable object avoidance using proximity sensors (ie. horizontal lidar)
AUXSW_ARMDISARM = 41, // arm or disarm vehicle
AUXSW_SMART_RTL = 42, // change to SmartRTL flight mode
AUXSW_INVERTED = 43, // enable inverted flight
AUXSW_SWITCH_MAX,
};

View File

@ -196,6 +196,7 @@ void Copter::init_aux_switch_function(int8_t ch_option, uint8_t ch_flag)
case AUXSW_AVOID_ADSB:
case AUXSW_PRECISION_LOITER:
case AUXSW_AVOID_PROXIMITY:
case AUXSW_INVERTED:
do_aux_switch_function(ch_option, ch_flag);
break;
}
@ -606,6 +607,23 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
}
}
break;
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;
}
#endif
break;
}
}