Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-06 07:53:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: added a new CRUISE flight mode

Browse Source 
See
http://diydrones.com/group/apmusergroup/forum/topics/feature-request-hold-heading-mode

this fixes issue #438
This commit is contained in:
Andrew Tridgell 2013-07-13 20:05:53 +10:00
parent 213472102c
commit 113bc20f8a
9 changed files with 139 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
ArduPlane/ArduPlane.pde
View File

@ -464,6 +464,15 @@ static struct {
int32_t locked_pitch_cd;
} acro_state;
////////////////////////////////////////////////////////////////////////////////
// CRUISE controller state
////////////////////////////////////////////////////////////////////////////////
static struct {
bool locked_heading;
int32_t locked_heading_cd;
uint32_t lock_timer_ms;
} cruise_state;
////////////////////////////////////////////////////////////////////////////////
// flight mode specific
////////////////////////////////////////////////////////////////////////////////
@ -1153,42 +1162,31 @@ static void update_flight_mode(void)
break;
}
case FLY_BY_WIRE_B: {
static float last_elevator_input;
// Substitute stick inputs for Navigation control output
// We use g.pitch_limit_min because its magnitude is
// normally greater than g.pitch_limit_max
case FLY_BY_WIRE_B:
// Thanks to Yury MonZon for the altitude limit code!
calc_nav_roll();
update_fbwb_speed_height();
break;
nav_roll_cd = channel_roll->norm_input() * g.roll_limit_cd;
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 (control_mode == CRUISE &&
(channel_roll->control_in != 0 ||
channel_rudder->control_in != 0)) {
cruise_state.locked_heading = false;
cruise_state.lock_timer_ms = 0;
}
float elevator_input;
elevator_input = channel_pitch->control_in / 4500.0f;
if (g.flybywire_elev_reverse) {
elevator_input = -elevator_input;
if (control_mode != CRUISE || !cruise_state.locked_heading) {
nav_roll_cd = channel_roll->norm_input() * g.roll_limit_cd;
} else {
calc_nav_roll();
}
target_altitude_cm += g.flybywire_climb_rate * elevator_input * delta_ms_fast_loop * 0.1f;
if (elevator_input == 0.0f && last_elevator_input != 0.0f) {
// the user has just released the elevator, lock in
// the current altitude
target_altitude_cm = current_loc.alt;
}
// check for FBWB altitude limit
if (g.FBWB_min_altitude_cm != 0 && target_altitude_cm < home.alt + g.FBWB_min_altitude_cm) {
target_altitude_cm = home.alt + g.FBWB_min_altitude_cm;
}
altitude_error_cm = target_altitude_cm - adjusted_altitude_cm();
last_elevator_input = elevator_input;
calc_throttle();
calc_nav_pitch();
}
update_fbwb_speed_height();
break;
case STABILIZE:
@ -1241,6 +1239,10 @@ static void update_navigation()
update_loiter();
break;
case CRUISE:
update_cruise();
break;
case MANUAL:
case STABILIZE:
case TRAINING:

6
ArduPlane/Attitude.pde
View File

@ -102,6 +102,7 @@ static void stabilize_stick_mixing_direct()
control_mode == ACRO ||
control_mode == FLY_BY_WIRE_A ||
control_mode == FLY_BY_WIRE_B ||
control_mode == CRUISE ||
control_mode == TRAINING) {
return;
}
@ -140,6 +141,7 @@ static void stabilize_stick_mixing_fbw()
control_mode == ACRO ||
control_mode == FLY_BY_WIRE_A ||
control_mode == FLY_BY_WIRE_B ||
control_mode == CRUISE ||
control_mode == TRAINING) {
return;
}
@ -883,8 +885,8 @@ static void set_servos(void)
static bool demoing_servos;
static void demo_servos(uint8_t i) {
static void demo_servos(uint8_t i)
{
while(i > 0) {
gcs_send_text_P(SEVERITY_LOW,PSTR("Demo Servos!"));
demoing_servos = true;

3
ArduPlane/GCS_Mavlink.pde
View File

@ -49,6 +49,7 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan)
case STABILIZE:
case FLY_BY_WIRE_A:
case FLY_BY_WIRE_B:
case CRUISE:
base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
break;
case AUTO:
@ -171,6 +172,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
break;
case FLY_BY_WIRE_B:
case CRUISE:
control_sensors_enabled |= (1<<10); // 3D angular rate control
control_sensors_enabled |= (1<<11); // attitude stabilisation
control_sensors_enabled |= (1<<15); // motor control
@ -1272,6 +1274,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case ACRO:
case FLY_BY_WIRE_A:
case FLY_BY_WIRE_B:
case CRUISE:
case AUTO:
case RTL:
case LOITER:

22
ArduPlane/Parameters.pde
View File

@ -250,7 +250,7 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Param: ARSPD_FBW_MIN
// @DisplayName: Fly By Wire Minimum Airspeed
// @Description: Airspeed corresponding to minimum throttle in Fly By Wire B mode.
// @Description: Airspeed corresponding to minimum throttle in auto throttle modes (FBWB, CRUISE, AUTO, GUIDED, LOITER, CIRCLE and RTL)
// @Units: m/s
// @Range: 5 50
// @Increment: 1
@ -259,7 +259,7 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Param: ARSPD_FBW_MAX
// @DisplayName: Fly By Wire Maximum Airspeed
// @Description: Airspeed corresponding to maximum throttle in Fly By Wire B mode.
// @Description: Airspeed corresponding to maximum throttle in auto throttle modes (FBWB, CRUISE, AUTO, GUIDED, LOITER, CIRCLE and RTL)
// @Units: m/s
// @Range: 5 50
// @Increment: 1
@ -268,14 +268,14 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Param: FBWB_ELEV_REV
// @DisplayName: Fly By Wire elevator reverse
// @Description: Reverse sense of elevator in FBWB. When set to 0 up elevator (pulling back on the stick) means to lower altitude. When set to 1, up elevator means to raise altitude.
// @Description: Reverse sense of elevator in FBWB and CRUISE modes. When set to 0 up elevator (pulling back on the stick) means to lower altitude. When set to 1, up elevator means to raise altitude.
// @Values: 0:Disabled,1:Enabled
// @User: Standard
GSCALAR(flybywire_elev_reverse, "FBWB_ELEV_REV", 0),
// @Param: FBWB_CLIMB_RATE
// @DisplayName: Fly By Wire B altitude change rate
// @Description: This sets the rate in m/s at which FBWB will change its target altitude for full elevator deflection. Note that the actual climb rate of the aircraft can be lower than this, depending on your airspeed and throttle control settings. If you have this parameter set to the default value of 2.0, then holding the elevator at maximum deflection for 10 seconds would change the target altitude by 20 meters.
// @Description: This sets the rate in m/s at which FBWB and CRUISE modes will change its target altitude for full elevator deflection. Note that the actual climb rate of the aircraft can be lower than this, depending on your airspeed and throttle control settings. If you have this parameter set to the default value of 2.0, then holding the elevator at maximum deflection for 10 seconds would change the target altitude by 20 meters.
// @Range: 1-10
// @Increment: 0.1
// @User: Standard
@ -416,7 +416,7 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Param: FLTMODE1
// @DisplayName: FlightMode1
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,10:Auto,11:RTL,12:Loiter,15:Guided
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,10:Auto,11:RTL,12:Loiter,15:Guided
// @User: Standard
// @Description: Flight mode for switch position 1 (910 to 1230 and above 2049)
GSCALAR(flight_mode1, "FLTMODE1", FLIGHT_MODE_1),
@ -424,35 +424,35 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Param: FLTMODE2
// @DisplayName: FlightMode2
// @Description: Flight mode for switch position 2 (1231 to 1360)
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,10:Auto,11:RTL,12:Loiter,15:Guided
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,10:Auto,11:RTL,12:Loiter,15:Guided
// @User: Standard
GSCALAR(flight_mode2, "FLTMODE2", FLIGHT_MODE_2),
// @Param: FLTMODE3
// @DisplayName: FlightMode3
// @Description: Flight mode for switch position 3 (1361 to 1490)
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,10:Auto,11:RTL,12:Loiter,15:Guided
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,10:Auto,11:RTL,12:Loiter,15:Guided
// @User: Standard
GSCALAR(flight_mode3, "FLTMODE3", FLIGHT_MODE_3),
// @Param: FLTMODE4
// @DisplayName: FlightMode4
// @Description: Flight mode for switch position 4 (1491 to 1620)
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,10:Auto,11:RTL,12:Loiter,15:Guided
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,10:Auto,11:RTL,12:Loiter,15:Guided
// @User: Standard
GSCALAR(flight_mode4, "FLTMODE4", FLIGHT_MODE_4),
// @Param: FLTMODE5
// @DisplayName: FlightMode5
// @Description: Flight mode for switch position 5 (1621 to 1749)
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,10:Auto,11:RTL,12:Loiter,15:Guided
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,10:Auto,11:RTL,12:Loiter,15:Guided
// @User: Standard
GSCALAR(flight_mode5, "FLTMODE5", FLIGHT_MODE_5),
// @Param: FLTMODE6
// @DisplayName: FlightMode6
// @Description: Flight mode for switch position 6 (1750 to 2049)
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,10:Auto,11:RTL,12:Loiter,15:Guided
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,10:Auto,11:RTL,12:Loiter,15:Guided
// @User: Standard
GSCALAR(flight_mode6, "FLTMODE6", FLIGHT_MODE_6),
@ -620,7 +620,7 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Param: ALT_HOLD_FBWCM
// @DisplayName: Minimum altitude for FBWB mode
// @Description: This is the minimum altitude in centimeters that FBWB will allow. If you attempt to descend below this altitude then the plane will level off. A value of zero means no limit.
// @Description: This is the minimum altitude in centimeters that FBWB and CRUISE modes will allow. If you attempt to descend below this altitude then the plane will level off. A value of zero means no limit.
// @Units: centimeters
// @User: User
GSCALAR(FBWB_min_altitude_cm, "ALT_HOLD_FBWCM", ALT_HOLD_FBW_CM),

1
ArduPlane/defines.h
View File

@ -61,6 +61,7 @@ enum FlightMode {
ACRO = 4,
FLY_BY_WIRE_A = 5,
FLY_BY_WIRE_B = 6,
CRUISE = 7,
AUTO = 10,
RTL = 11,
LOITER = 12,

2
ArduPlane/events.pde
View File

@ -14,6 +14,7 @@ static void failsafe_short_on_event(int16_t fstype)
case ACRO:
case FLY_BY_WIRE_A:
case FLY_BY_WIRE_B:
case CRUISE:
case TRAINING:
set_mode(CIRCLE);
break;
@ -48,6 +49,7 @@ static void failsafe_long_on_event(int16_t fstype)
case ACRO:
case FLY_BY_WIRE_A:
case FLY_BY_WIRE_B:
case CRUISE:
case TRAINING:
case CIRCLE:
set_mode(RTL);

76
ArduPlane/navigation.pde
View File

@ -84,7 +84,8 @@ static void calc_airspeed_errors()
target_airspeed_cm = g.airspeed_cruise_cm;
// FBW_B airspeed target
if (control_mode == FLY_BY_WIRE_B) {
if (control_mode == FLY_BY_WIRE_B ||
control_mode == CRUISE) {
target_airspeed_cm = ((int32_t)(aparm.flybywire_airspeed_max -
aparm.flybywire_airspeed_min) *
channel_throttle->control_in) +
@ -129,7 +130,8 @@ static void calc_gndspeed_undershoot()
static void calc_altitude_error()
{
if (control_mode == FLY_BY_WIRE_B) {
if (control_mode == FLY_BY_WIRE_B ||
control_mode == CRUISE) {
return;
}
if (offset_altitude_cm != 0) {
@ -154,6 +156,76 @@ static void update_loiter()
nav_controller->update_loiter(next_WP, abs(g.loiter_radius), loiter.direction);
}
/*
handle CRUISE mode, locking heading to GPS course when we have
sufficient ground speed, and no aileron or rudder input
*/
static void update_cruise()
{
if (!cruise_state.locked_heading &&
channel_roll->control_in == 0 &&
channel_rudder->control_in == 0 &&
g_gps && g_gps->status() >= GPS::GPS_OK_FIX_2D &&
g_gps->ground_speed_cm >= 300 &&
cruise_state.lock_timer_ms == 0) {
// user wants to lock the heading - start the timer
cruise_state.lock_timer_ms = hal.scheduler->millis();
}
if (cruise_state.lock_timer_ms != 0 &&
(hal.scheduler->millis() - cruise_state.lock_timer_ms) > 500) {
// lock the heading after 0.5 seconds of zero heading input
// from user
cruise_state.locked_heading = true;
cruise_state.lock_timer_ms = 0;
cruise_state.locked_heading_cd = g_gps->ground_course_cd;
prev_WP = current_loc;
}
if (cruise_state.locked_heading) {
next_WP = prev_WP;
// always look 1km ahead
location_update(&next_WP,
cruise_state.locked_heading_cd*0.01f,
get_distance(&prev_WP, &current_loc) + 1000);
nav_controller->update_waypoint(prev_WP, next_WP);
}
}
/*
handle speed and height control in FBWB or CRUISE mode.
In this mode the elevator is used to change target altitude. The
throttle is used to change target airspeed or throttle
*/
static void update_fbwb_speed_height(void)
{
static float last_elevator_input;
float elevator_input;
elevator_input = channel_pitch->control_in / 4500.0f;
if (g.flybywire_elev_reverse) {
elevator_input = -elevator_input;
}
target_altitude_cm += g.flybywire_climb_rate * elevator_input * delta_ms_fast_loop * 0.1f;
if (elevator_input == 0.0f && last_elevator_input != 0.0f) {
// the user has just released the elevator, lock in
// the current altitude
target_altitude_cm = current_loc.alt;
}
// check for FBWB altitude limit
if (g.FBWB_min_altitude_cm != 0 && target_altitude_cm < home.alt + g.FBWB_min_altitude_cm) {
target_altitude_cm = home.alt + g.FBWB_min_altitude_cm;
}
altitude_error_cm = target_altitude_cm - adjusted_altitude_cm();
last_elevator_input = elevator_input;
calc_throttle();
calc_nav_pitch();
}
static void setup_glide_slope(void)
{
// establish the distance we are travelling to the next waypoint,

1
ArduPlane/setup.pde
View File

@ -313,6 +313,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
mode != ACRO &&
mode != FLY_BY_WIRE_A &&
mode != FLY_BY_WIRE_B &&
mode != CRUISE &&
mode != AUTO &&
mode != RTL &&
mode != LOITER)

9
ArduPlane/system.pde
View File

@ -348,6 +348,12 @@ static void set_mode(enum FlightMode mode)
acro_state.locked_pitch = false;
break;
case CRUISE:
cruise_state.locked_heading = false;
cruise_state.lock_timer_ms = 0;
target_altitude_cm = current_loc.alt;
break;
case FLY_BY_WIRE_B:
target_altitude_cm = current_loc.alt;
break;
@ -633,6 +639,9 @@ print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
case FLY_BY_WIRE_B:
port->print_P(PSTR("FBW_B"));
break;
case CRUISE:
port->print_P(PSTR("CRUISE"));
break;
case AUTO:
port->print_P(PSTR("AUTO"));
break;