Plane: added MAV_CMD_DO_INVERTED_FLIGHT support

allows for mission control over inverted flight
This commit is contained in:
Andrew Tridgell 2014-06-05 16:12:10 +10:00
parent f0df912a11
commit ea086fa79c
5 changed files with 35 additions and 5 deletions

View File

@ -536,6 +536,9 @@ static struct {
// turn angle for next leg of mission
float next_turn_angle;
// should we fly inverted?
bool inverted_flight;
} auto_state = {
takeoff_complete : true,
land_complete : false,
@ -543,7 +546,8 @@ static struct {
takeoff_pitch_cd : 0,
highest_airspeed : 0,
initial_pitch_cd : 0,
next_turn_angle : 90.0f
next_turn_angle : 90.0f,
inverted_flight : false
};
// true if we are in an auto-throttle mode, which means
@ -1218,7 +1222,7 @@ static void update_flight_mode(void)
training_manual_pitch = true;
nav_pitch_cd = 0;
}
if (inverted_flight) {
if (fly_inverted()) {
nav_pitch_cd = -nav_pitch_cd;
}
break;
@ -1251,7 +1255,7 @@ static void update_flight_mode(void)
nav_pitch_cd = -(pitch_input * pitch_limit_min_cd);
}
nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get());
if (inverted_flight) {
if (fly_inverted()) {
nav_pitch_cd = -nav_pitch_cd;
}
if (failsafe.ch3_failsafe && g.short_fs_action == 2) {

View File

@ -71,7 +71,7 @@ static bool stick_mixing_enabled(void)
*/
static void stabilize_roll(float speed_scaler)
{
if (inverted_flight) {
if (fly_inverted()) {
// we want to fly upside down. We need to cope with wrap of
// the roll_sensor interfering with wrap of nav_roll, which
// would really confuse the PID code. The easiest way to
@ -185,7 +185,7 @@ static void stabilize_stick_mixing_fbw()
if (fabsf(pitch_input) > 0.5f) {
pitch_input = (3*pitch_input - 1);
}
if (inverted_flight) {
if (fly_inverted()) {
pitch_input = -pitch_input;
}
if (pitch_input > 0) {

View File

@ -114,6 +114,13 @@ start_command(const AP_Mission::Mission_Command& cmd)
cmd.content.repeat_relay.cycle_time * 1000.0f);
break;
case MAV_CMD_DO_INVERTED_FLIGHT:
if (cmd.p1 == 0 || cmd.p1 == 1) {
auto_state.inverted_flight = (bool)cmd.p1;
gcs_send_text_fmt(PSTR("Set inverted %u"), cmd.p1);
}
break;
#if CAMERA == ENABLED
case MAV_CMD_DO_CONTROL_VIDEO: // Control on-board camera capturing. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty|
break;
@ -219,6 +226,7 @@ static bool verify_command(const AP_Mission::Mission_Command& cmd) // Ret
case MAV_CMD_NAV_ROI:
case MAV_CMD_DO_MOUNT_CONFIGURE:
case MAV_CMD_DO_MOUNT_CONTROL:
case MAV_CMD_DO_INVERTED_FLIGHT:
return true;
default:

View File

@ -96,3 +96,18 @@ static void autotune_restore(void)
rollController.autotune_restore();
pitchController.autotune_restore();
}
/*
are we flying inverted?
*/
static bool fly_inverted(void)
{
if (g.inverted_flight_ch != 0 && inverted_flight) {
// controlled with INVERTED_FLIGHT_CH
return true;
}
if (control_mode == AUTO && auto_state.inverted_flight) {
return true;
}
return false;
}

View File

@ -282,6 +282,9 @@ static void set_mode(enum FlightMode mode)
// perform any cleanup required for prev flight mode
exit_mode(control_mode);
// cancel inverted flight
auto_state.inverted_flight = false;
// set mode
previous_mode = control_mode;
control_mode = mode;