mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
Plane: added MAV_CMD_DO_INVERTED_FLIGHT support
allows for mission control over inverted flight
This commit is contained in:
parent
f0df912a11
commit
ea086fa79c
@ -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) {
|
||||
|
@ -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) {
|
||||
|
@ -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:
|
||||
|
@ -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;
|
||||
}
|
||||
|
@ -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;
|
||||
|
Loading…
Reference in New Issue
Block a user