zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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
master
Andrew Tridgell 12 years ago
parent
98a2ffd65c
commit
cc6acac1dd
4 changed files with 208 additions and 183 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 380
      ArduPlane/ArduPlane.pde
  2. 2
      ArduPlane/Attitude.pde
  3. 2
      ArduPlane/Parameters.h
  4. 7
      ArduPlane/Parameters.pde

380
ArduPlane/ArduPlane.pde
Unescape View File

@ -1054,206 +1054,222 @@ static void update_GPS(void) @@ -1054,206 +1054,222 @@ static void update_GPS(void)
calc_gndspeed_undershoot();
}
static void update_flight_mode(void)
/*
main handling for AUTO mode
*/
static void handle_auto_mode(void)
{
if(control_mode == AUTO) {
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:
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;
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);
}
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();
}
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;
}
}
if (land_complete) {
// we are in the final stage of a landing - force
// zero throttle
channel_throttle->servo_out = 0;
}
break;
/*
main flight mode dependent update code
*/
static void update_flight_mode(void)
{
enum FlightMode effective_mode = control_mode;
if (control_mode == AUTO && g.auto_fbw_steer) {
effective_mode = FLY_BY_WIRE_A;
}
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{
if (effective_mode != AUTO) {
// hold_course is only used in takeoff and landing
hold_course_cd = -1;
}
switch(control_mode) {
case RTL:
case LOITER:
case GUIDED:
calc_nav_roll();
calc_nav_pitch();
calc_throttle();
break;
case TRAINING: {
training_manual_roll = false;
training_manual_pitch = false;
// 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) {
nav_roll_cd = g.roll_limit_cd;
} else if (ahrs.roll_sensor <= -g.roll_limit_cd) {
nav_roll_cd = -g.roll_limit_cd;
} else {
training_manual_roll = true;
nav_roll_cd = 0;
}
switch (effective_mode)
{
case AUTO:
handle_auto_mode();
break;
// if the pitch is past the set pitch limits, then
// we set target pitch to the limit
if (ahrs.pitch_sensor >= aparm.pitch_limit_max_cd) {
nav_pitch_cd = aparm.pitch_limit_max_cd;
} else if (ahrs.pitch_sensor <= aparm.pitch_limit_min_cd) {
nav_pitch_cd = aparm.pitch_limit_min_cd;
} else {
training_manual_pitch = true;
nav_pitch_cd = 0;
}
if (inverted_flight) {
nav_pitch_cd = -nav_pitch_cd;
}
break;
case RTL:
case LOITER:
case GUIDED:
calc_nav_roll();
calc_nav_pitch();
calc_throttle();
break;
case TRAINING: {
training_manual_roll = false;
training_manual_pitch = false;
// 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) {
nav_roll_cd = g.roll_limit_cd;
} else if (ahrs.roll_sensor <= -g.roll_limit_cd) {
nav_roll_cd = -g.roll_limit_cd;
} else {
training_manual_roll = true;
nav_roll_cd = 0;
}
// if the pitch is past the set pitch limits, then
// we set target pitch to the limit
if (ahrs.pitch_sensor >= aparm.pitch_limit_max_cd) {
nav_pitch_cd = aparm.pitch_limit_max_cd;
} else if (ahrs.pitch_sensor <= aparm.pitch_limit_min_cd) {
nav_pitch_cd = aparm.pitch_limit_min_cd;
} else {
training_manual_pitch = true;
nav_pitch_cd = 0;
}
if (inverted_flight) {
nav_pitch_cd = -nav_pitch_cd;
}
break;
}
case ACRO: {
// 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 ACRO: {
// 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_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!
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;
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;
} 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;
} 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:
// handled elsewhere
break;
}
}

2
ArduPlane/Attitude.pde
Unescape View File

@ -533,7 +533,7 @@ static bool suppress_throttle(void) @@ -533,7 +533,7 @@ static bool suppress_throttle(void)
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
throttle_suppressed = false;
if (hold_course_cd != -1) {

2
ArduPlane/Parameters.h
Unescape View File

@ -92,6 +92,7 @@ public: @@ -92,6 +92,7 @@ public:
k_param_relay,
k_param_takeoff_throttle_delay,
k_param_skip_gyro_cal,
k_param_auto_fbw_steer,
// 110: Telemetry control
//
@ -281,6 +282,7 @@ public: @@ -281,6 +282,7 @@ public:
// skip gyro calibration
AP_Int8 skip_gyro_cal;
AP_Int8 auto_fbw_steer;
// Estimation
//

7
ArduPlane/Parameters.pde
Unescape View File

@ -81,6 +81,13 @@ const AP_Param::Info var_info[] PROGMEM = { @@ -81,6 +81,13 @@ const AP_Param::Info var_info[] PROGMEM = {
// @User: Advanced
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
// @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.

Write Preview
Loading…
Cancel
Save