mirror of https://github.com/ArduPilot/ardupilot
Plane: added AUTO_FBW_STEER option
this is an unusual option that gives FBWA flight control in AUTO mode. It is being added to support use of APM in an aerial robotics competition where students need to pilot the plane, but they still need waypoint triggering of payloads
This commit is contained in:
Andrew Tridgell
parent
98a2ffd65c
commit
cc6acac1dd
|
|
@ -1054,206 +1054,222 @@ static void update_GPS(void)
|
||||||
calc_gndspeed_undershoot();
|
calc_gndspeed_undershoot();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
main handling for AUTO mode
|
||||||
|
*/
|
||||||
|
static void handle_auto_mode(void)
|
||||||
|
{
|
||||||
|
switch(nav_command_ID) {
|
||||||
|
case MAV_CMD_NAV_TAKEOFF:
|
||||||
|
if (hold_course_cd == -1) {
|
||||||
|
// we don't yet have a heading to hold - just level
|
||||||
|
// the wings until we get up enough speed to get a GPS heading
|
||||||
|
nav_roll_cd = 0;
|
||||||
|
} else {
|
||||||
|
calc_nav_roll();
|
||||||
|
// during takeoff use the level flight roll limit to
|
||||||
|
// prevent large course corrections
|
||||||
|
nav_roll_cd = constrain_int32(nav_roll_cd, -g.level_roll_limit*100UL, g.level_roll_limit*100UL);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (airspeed.use()) {
|
||||||
|
calc_nav_pitch();
|
||||||
|
if (nav_pitch_cd < takeoff_pitch_cd)
|
||||||
|
nav_pitch_cd = takeoff_pitch_cd;
|
||||||
|
} else {
|
||||||
|
nav_pitch_cd = (g_gps->ground_speed_cm / (float)g.airspeed_cruise_cm) * takeoff_pitch_cd;
|
||||||
|
nav_pitch_cd = constrain_int32(nav_pitch_cd, 500, takeoff_pitch_cd);
|
||||||
|
}
|
||||||
|
|
||||||
|
// max throttle for takeoff
|
||||||
|
channel_throttle->servo_out = aparm.throttle_max;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MAV_CMD_NAV_LAND:
|
||||||
|
calc_nav_roll();
|
||||||
|
|
||||||
|
if (land_complete) {
|
||||||
|
// during final approach constrain roll to the range
|
||||||
|
// allowed for level flight
|
||||||
|
nav_roll_cd = constrain_int32(nav_roll_cd, -g.level_roll_limit*100UL, g.level_roll_limit*100UL);
|
||||||
|
|
||||||
|
// hold pitch constant in final approach
|
||||||
|
nav_pitch_cd = g.land_pitch_cd;
|
||||||
|
} else {
|
||||||
|
calc_nav_pitch();
|
||||||
|
if (!airspeed.use()) {
|
||||||
|
// when not under airspeed control, don't allow
|
||||||
|
// down pitch in landing
|
||||||
|
nav_pitch_cd = constrain_int32(nav_pitch_cd, 0, nav_pitch_cd);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
calc_throttle();
|
||||||
|
|
||||||
|
if (land_complete) {
|
||||||
|
// we are in the final stage of a landing - force
|
||||||
|
// zero throttle
|
||||||
|
channel_throttle->servo_out = 0;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
// we are doing normal AUTO flight, the special cases
|
||||||
|
// are for takeoff and landing
|
||||||
|
hold_course_cd = -1;
|
||||||
|
land_complete = false;
|
||||||
|
calc_nav_roll();
|
||||||
|
calc_nav_pitch();
|
||||||
|
calc_throttle();
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
main flight mode dependent update code
|
||||||
|
*/
|
||||||
static void update_flight_mode(void)
|
static void update_flight_mode(void)
|
||||||
{
|
{
|
||||||
if(control_mode == AUTO) {
|
enum FlightMode effective_mode = control_mode;
|
||||||
|
if (control_mode == AUTO && g.auto_fbw_steer) {
|
||||||
|
effective_mode = FLY_BY_WIRE_A;
|
||||||
|
}
|
||||||
|
|
||||||
switch(nav_command_ID) {
|
if (effective_mode != AUTO) {
|
||||||
case MAV_CMD_NAV_TAKEOFF:
|
|
||||||
if (hold_course_cd == -1) {
|
|
||||||
// we don't yet have a heading to hold - just level
|
|
||||||
// the wings until we get up enough speed to get a GPS heading
|
|
||||||
nav_roll_cd = 0;
|
|
||||||
} else {
|
|
||||||
calc_nav_roll();
|
|
||||||
// during takeoff use the level flight roll limit to
|
|
||||||
// prevent large course corrections
|
|
||||||
nav_roll_cd = constrain_int32(nav_roll_cd, -g.level_roll_limit*100UL, g.level_roll_limit*100UL);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (airspeed.use()) {
|
|
||||||
calc_nav_pitch();
|
|
||||||
if (nav_pitch_cd < takeoff_pitch_cd)
|
|
||||||
nav_pitch_cd = takeoff_pitch_cd;
|
|
||||||
} else {
|
|
||||||
nav_pitch_cd = (g_gps->ground_speed_cm / (float)g.airspeed_cruise_cm) * takeoff_pitch_cd;
|
|
||||||
nav_pitch_cd = constrain_int32(nav_pitch_cd, 500, takeoff_pitch_cd);
|
|
||||||
}
|
|
||||||
|
|
||||||
// max throttle for takeoff
|
|
||||||
channel_throttle->servo_out = aparm.throttle_max;
|
|
||||||
|
|
||||||
break;
|
|
||||||
|
|
||||||
case MAV_CMD_NAV_LAND:
|
|
||||||
calc_nav_roll();
|
|
||||||
|
|
||||||
if (land_complete) {
|
|
||||||
// during final approach constrain roll to the range
|
|
||||||
// allowed for level flight
|
|
||||||
nav_roll_cd = constrain_int32(nav_roll_cd, -g.level_roll_limit*100UL, g.level_roll_limit*100UL);
|
|
||||||
|
|
||||||
// hold pitch constant in final approach
|
|
||||||
nav_pitch_cd = g.land_pitch_cd;
|
|
||||||
} else {
|
|
||||||
calc_nav_pitch();
|
|
||||||
if (!airspeed.use()) {
|
|
||||||
// when not under airspeed control, don't allow
|
|
||||||
// down pitch in landing
|
|
||||||
nav_pitch_cd = constrain_int32(nav_pitch_cd, 0, nav_pitch_cd);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
calc_throttle();
|
|
||||||
|
|
||||||
if (land_complete) {
|
|
||||||
// we are in the final stage of a landing - force
|
|
||||||
// zero throttle
|
|
||||||
channel_throttle->servo_out = 0;
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
|
|
||||||
default:
|
|
||||||
// we are doing normal AUTO flight, the special cases
|
|
||||||
// are for takeoff and landing
|
|
||||||
hold_course_cd = -1;
|
|
||||||
land_complete = false;
|
|
||||||
calc_nav_roll();
|
|
||||||
calc_nav_pitch();
|
|
||||||
calc_throttle();
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}else{
|
|
||||||
// hold_course is only used in takeoff and landing
|
// hold_course is only used in takeoff and landing
|
||||||
hold_course_cd = -1;
|
hold_course_cd = -1;
|
||||||
|
}
|
||||||
|
|
||||||
switch(control_mode) {
|
switch (effective_mode)
|
||||||
case RTL:
|
{
|
||||||
case LOITER:
|
case AUTO:
|
||||||
case GUIDED:
|
handle_auto_mode();
|
||||||
calc_nav_roll();
|
break;
|
||||||
calc_nav_pitch();
|
|
||||||
calc_throttle();
|
|
||||||
break;
|
|
||||||
|
|
||||||
case TRAINING: {
|
case RTL:
|
||||||
training_manual_roll = false;
|
case LOITER:
|
||||||
training_manual_pitch = false;
|
case GUIDED:
|
||||||
|
calc_nav_roll();
|
||||||
// if the roll is past the set roll limit, then
|
calc_nav_pitch();
|
||||||
// we set target roll to the limit
|
calc_throttle();
|
||||||
if (ahrs.roll_sensor >= g.roll_limit_cd) {
|
break;
|
||||||
nav_roll_cd = g.roll_limit_cd;
|
|
||||||
} else if (ahrs.roll_sensor <= -g.roll_limit_cd) {
|
case TRAINING: {
|
||||||
nav_roll_cd = -g.roll_limit_cd;
|
training_manual_roll = false;
|
||||||
} else {
|
training_manual_pitch = false;
|
||||||
training_manual_roll = true;
|
|
||||||
nav_roll_cd = 0;
|
// if the roll is past the set roll limit, then
|
||||||
}
|
// we set target roll to the limit
|
||||||
|
if (ahrs.roll_sensor >= g.roll_limit_cd) {
|
||||||
// if the pitch is past the set pitch limits, then
|
nav_roll_cd = g.roll_limit_cd;
|
||||||
// we set target pitch to the limit
|
} else if (ahrs.roll_sensor <= -g.roll_limit_cd) {
|
||||||
if (ahrs.pitch_sensor >= aparm.pitch_limit_max_cd) {
|
nav_roll_cd = -g.roll_limit_cd;
|
||||||
nav_pitch_cd = aparm.pitch_limit_max_cd;
|
} else {
|
||||||
} else if (ahrs.pitch_sensor <= aparm.pitch_limit_min_cd) {
|
training_manual_roll = true;
|
||||||
nav_pitch_cd = aparm.pitch_limit_min_cd;
|
nav_roll_cd = 0;
|
||||||
} else {
|
|
||||||
training_manual_pitch = true;
|
|
||||||
nav_pitch_cd = 0;
|
|
||||||
}
|
|
||||||
if (inverted_flight) {
|
|
||||||
nav_pitch_cd = -nav_pitch_cd;
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
case ACRO: {
|
// if the pitch is past the set pitch limits, then
|
||||||
// handle locked/unlocked control
|
// we set target pitch to the limit
|
||||||
if (acro_state.locked_roll) {
|
if (ahrs.pitch_sensor >= aparm.pitch_limit_max_cd) {
|
||||||
nav_roll_cd = acro_state.locked_roll_err;
|
nav_pitch_cd = aparm.pitch_limit_max_cd;
|
||||||
} else {
|
} else if (ahrs.pitch_sensor <= aparm.pitch_limit_min_cd) {
|
||||||
nav_roll_cd = ahrs.roll_sensor;
|
nav_pitch_cd = aparm.pitch_limit_min_cd;
|
||||||
}
|
} else {
|
||||||
if (acro_state.locked_pitch) {
|
training_manual_pitch = true;
|
||||||
nav_pitch_cd = acro_state.locked_pitch_cd;
|
nav_pitch_cd = 0;
|
||||||
} else {
|
|
||||||
nav_pitch_cd = ahrs.pitch_sensor;
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
if (inverted_flight) {
|
||||||
case FLY_BY_WIRE_A: {
|
nav_pitch_cd = -nav_pitch_cd;
|
||||||
// set nav_roll and nav_pitch using sticks
|
|
||||||
nav_roll_cd = channel_roll->norm_input() * g.roll_limit_cd;
|
|
||||||
nav_roll_cd = constrain_int32(nav_roll_cd, -g.roll_limit_cd, g.roll_limit_cd);
|
|
||||||
float pitch_input = channel_pitch->norm_input();
|
|
||||||
if (pitch_input > 0) {
|
|
||||||
nav_pitch_cd = pitch_input * aparm.pitch_limit_max_cd;
|
|
||||||
} else {
|
|
||||||
nav_pitch_cd = -(pitch_input * aparm.pitch_limit_min_cd);
|
|
||||||
}
|
|
||||||
nav_pitch_cd = constrain_int32(nav_pitch_cd, aparm.pitch_limit_min_cd.get(), aparm.pitch_limit_max_cd.get());
|
|
||||||
if (inverted_flight) {
|
|
||||||
nav_pitch_cd = -nav_pitch_cd;
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
case FLY_BY_WIRE_B:
|
case ACRO: {
|
||||||
// Thanks to Yury MonZon for the altitude limit code!
|
// handle locked/unlocked control
|
||||||
|
if (acro_state.locked_roll) {
|
||||||
|
nav_roll_cd = acro_state.locked_roll_err;
|
||||||
|
} else {
|
||||||
|
nav_roll_cd = ahrs.roll_sensor;
|
||||||
|
}
|
||||||
|
if (acro_state.locked_pitch) {
|
||||||
|
nav_pitch_cd = acro_state.locked_pitch_cd;
|
||||||
|
} else {
|
||||||
|
nav_pitch_cd = ahrs.pitch_sensor;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
case FLY_BY_WIRE_A: {
|
||||||
|
// set nav_roll and nav_pitch using sticks
|
||||||
|
nav_roll_cd = channel_roll->norm_input() * g.roll_limit_cd;
|
||||||
|
nav_roll_cd = constrain_int32(nav_roll_cd, -g.roll_limit_cd, g.roll_limit_cd);
|
||||||
|
float pitch_input = channel_pitch->norm_input();
|
||||||
|
if (pitch_input > 0) {
|
||||||
|
nav_pitch_cd = pitch_input * aparm.pitch_limit_max_cd;
|
||||||
|
} else {
|
||||||
|
nav_pitch_cd = -(pitch_input * aparm.pitch_limit_min_cd);
|
||||||
|
}
|
||||||
|
nav_pitch_cd = constrain_int32(nav_pitch_cd, aparm.pitch_limit_min_cd.get(), aparm.pitch_limit_max_cd.get());
|
||||||
|
if (inverted_flight) {
|
||||||
|
nav_pitch_cd = -nav_pitch_cd;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
case FLY_BY_WIRE_B:
|
||||||
|
// Thanks to Yury MonZon for the altitude limit code!
|
||||||
|
nav_roll_cd = channel_roll->norm_input() * g.roll_limit_cd;
|
||||||
|
update_fbwb_speed_height();
|
||||||
|
break;
|
||||||
|
|
||||||
|
case CRUISE:
|
||||||
|
/*
|
||||||
|
in CRUISE mode we use the navigation code to control
|
||||||
|
roll when heading is locked. Heading becomes unlocked on
|
||||||
|
any aileron or rudder input
|
||||||
|
*/
|
||||||
|
if ((channel_roll->control_in != 0 ||
|
||||||
|
channel_rudder->control_in != 0)) {
|
||||||
|
cruise_state.locked_heading = false;
|
||||||
|
cruise_state.lock_timer_ms = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!cruise_state.locked_heading) {
|
||||||
nav_roll_cd = channel_roll->norm_input() * g.roll_limit_cd;
|
nav_roll_cd = channel_roll->norm_input() * g.roll_limit_cd;
|
||||||
update_fbwb_speed_height();
|
} else {
|
||||||
break;
|
calc_nav_roll();
|
||||||
|
|
||||||
case CRUISE:
|
|
||||||
/*
|
|
||||||
in CRUISE mode we use the navigation code to control
|
|
||||||
roll when heading is locked. Heading becomes unlocked on
|
|
||||||
any aileron or rudder input
|
|
||||||
*/
|
|
||||||
if ((channel_roll->control_in != 0 ||
|
|
||||||
channel_rudder->control_in != 0)) {
|
|
||||||
cruise_state.locked_heading = false;
|
|
||||||
cruise_state.lock_timer_ms = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!cruise_state.locked_heading) {
|
|
||||||
nav_roll_cd = channel_roll->norm_input() * g.roll_limit_cd;
|
|
||||||
} else {
|
|
||||||
calc_nav_roll();
|
|
||||||
}
|
|
||||||
update_fbwb_speed_height();
|
|
||||||
break;
|
|
||||||
|
|
||||||
case STABILIZE:
|
|
||||||
nav_roll_cd = 0;
|
|
||||||
nav_pitch_cd = 0;
|
|
||||||
// throttle is passthrough
|
|
||||||
break;
|
|
||||||
|
|
||||||
case CIRCLE:
|
|
||||||
// we have no GPS installed and have lost radio contact
|
|
||||||
// or we just want to fly around in a gentle circle w/o GPS,
|
|
||||||
// holding altitude at the altitude we set when we
|
|
||||||
// switched into the mode
|
|
||||||
nav_roll_cd = g.roll_limit_cd / 3;
|
|
||||||
calc_nav_pitch();
|
|
||||||
calc_throttle();
|
|
||||||
break;
|
|
||||||
|
|
||||||
case MANUAL:
|
|
||||||
// servo_out is for Sim control only
|
|
||||||
// ---------------------------------
|
|
||||||
channel_roll->servo_out = channel_roll->pwm_to_angle();
|
|
||||||
channel_pitch->servo_out = channel_pitch->pwm_to_angle();
|
|
||||||
channel_rudder->servo_out = channel_rudder->pwm_to_angle();
|
|
||||||
break;
|
|
||||||
//roll: -13788.000, pitch: -13698.000, thr: 0.000, rud: -13742.000
|
|
||||||
|
|
||||||
case INITIALISING:
|
|
||||||
case AUTO:
|
|
||||||
// handled elsewhere
|
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
update_fbwb_speed_height();
|
||||||
|
break;
|
||||||
|
|
||||||
|
case STABILIZE:
|
||||||
|
nav_roll_cd = 0;
|
||||||
|
nav_pitch_cd = 0;
|
||||||
|
// throttle is passthrough
|
||||||
|
break;
|
||||||
|
|
||||||
|
case CIRCLE:
|
||||||
|
// we have no GPS installed and have lost radio contact
|
||||||
|
// or we just want to fly around in a gentle circle w/o GPS,
|
||||||
|
// holding altitude at the altitude we set when we
|
||||||
|
// switched into the mode
|
||||||
|
nav_roll_cd = g.roll_limit_cd / 3;
|
||||||
|
calc_nav_pitch();
|
||||||
|
calc_throttle();
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MANUAL:
|
||||||
|
// servo_out is for Sim control only
|
||||||
|
// ---------------------------------
|
||||||
|
channel_roll->servo_out = channel_roll->pwm_to_angle();
|
||||||
|
channel_pitch->servo_out = channel_pitch->pwm_to_angle();
|
||||||
|
channel_rudder->servo_out = channel_rudder->pwm_to_angle();
|
||||||
|
break;
|
||||||
|
//roll: -13788.000, pitch: -13698.000, thr: 0.000, rud: -13742.000
|
||||||
|
|
||||||
|
case INITIALISING:
|
||||||
|
// handled elsewhere
|
||||||
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -533,7 +533,7 @@ static bool suppress_throttle(void)
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (control_mode==AUTO && takeoff_complete == false && auto_takeoff_check()) {
|
if (control_mode==AUTO && !g.auto_fbw_steer && takeoff_complete == false && auto_takeoff_check()) {
|
||||||
// we're in auto takeoff
|
// we're in auto takeoff
|
||||||
throttle_suppressed = false;
|
throttle_suppressed = false;
|
||||||
if (hold_course_cd != -1) {
|
if (hold_course_cd != -1) {
|
||||||
|
|
|
||||||
|
|
@ -92,6 +92,7 @@ public:
|
||||||
k_param_relay,
|
k_param_relay,
|
||||||
k_param_takeoff_throttle_delay,
|
k_param_takeoff_throttle_delay,
|
||||||
k_param_skip_gyro_cal,
|
k_param_skip_gyro_cal,
|
||||||
|
k_param_auto_fbw_steer,
|
||||||
|
|
||||||
// 110: Telemetry control
|
// 110: Telemetry control
|
||||||
//
|
//
|
||||||
|
|
@ -281,6 +282,7 @@ public:
|
||||||
|
|
||||||
// skip gyro calibration
|
// skip gyro calibration
|
||||||
AP_Int8 skip_gyro_cal;
|
AP_Int8 skip_gyro_cal;
|
||||||
|
AP_Int8 auto_fbw_steer;
|
||||||
|
|
||||||
// Estimation
|
// Estimation
|
||||||
//
|
//
|
||||||
|
|
|
||||||
|
|
@ -81,6 +81,13 @@ const AP_Param::Info var_info[] PROGMEM = {
|
||||||
// @User: Advanced
|
// @User: Advanced
|
||||||
GSCALAR(skip_gyro_cal, "SKIP_GYRO_CAL", 0),
|
GSCALAR(skip_gyro_cal, "SKIP_GYRO_CAL", 0),
|
||||||
|
|
||||||
|
// @Param: AUTO_FBW_STEER
|
||||||
|
// @DisplayName: Use FBWA steering in AUTO
|
||||||
|
// @Description: When enabled this option gives FBWA navigation and steering in AUTO mode. This can be used to allow manual stabilised piloting with waypoint logic for triggering payloads. With this enabled the pilot has the same control over the plane as in FBWA mode, and the normal AUTO navigation is completely disabled. This option is not recommended for normal use.
|
||||||
|
// @Values: 0:Disabled,1:Enabled
|
||||||
|
// @User: Advanced
|
||||||
|
GSCALAR(auto_fbw_steer, "AUTO_FBW_STEER", 0),
|
||||||
|
|
||||||
// @Param: TKOFF_THR_MINSPD
|
// @Param: TKOFF_THR_MINSPD
|
||||||
// @DisplayName: Takeoff throttle min speed
|
// @DisplayName: Takeoff throttle min speed
|
||||||
// @Description: Minimum GPS ground speed in m/s used by the speed check that un-suppresses throttle in auto-takeoff. This can be be used for catapult launches where you want the motor to engage only after the plane leaves the catapult, but it is preferable to use the TKOFF_THR_MINACC and TKOFF_THR_DELAY parameters for cvatapult launches due to the errors associated with GPS measurements. For hand launches with a pusher prop it is strongly advised that this parameter be set to a value no less than 4 m/s to provide additional protection against premature motor start. Note that the GPS velocity will lag the real velocity by about 0.5 seconds. The ground speed check is delayed by the TKOFF_THR_DELAY parameter.
|
// @Description: Minimum GPS ground speed in m/s used by the speed check that un-suppresses throttle in auto-takeoff. This can be be used for catapult launches where you want the motor to engage only after the plane leaves the catapult, but it is preferable to use the TKOFF_THR_MINACC and TKOFF_THR_DELAY parameters for cvatapult launches due to the errors associated with GPS measurements. For hand launches with a pusher prop it is strongly advised that this parameter be set to a value no less than 4 m/s to provide additional protection against premature motor start. Note that the GPS velocity will lag the real velocity by about 0.5 seconds. The ground speed check is delayed by the TKOFF_THR_DELAY parameter.
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue