From 113bc20f8a9a410e639a18adb2df9174525928a4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 13 Jul 2013 20:05:53 +1000 Subject: [PATCH] Plane: added a new CRUISE flight mode See http://diydrones.com/group/apmusergroup/forum/topics/feature-request-hold-heading-mode this fixes issue #438 --- ArduPlane/ArduPlane.pde | 66 +++++++++++++++++----------------- ArduPlane/Attitude.pde | 6 ++-- ArduPlane/GCS_Mavlink.pde | 3 ++ ArduPlane/Parameters.pde | 22 ++++++------ ArduPlane/defines.h | 1 + ArduPlane/events.pde | 2 ++ ArduPlane/navigation.pde | 76 +++++++++++++++++++++++++++++++++++++-- ArduPlane/setup.pde | 1 + ArduPlane/system.pde | 9 +++++ 9 files changed, 139 insertions(+), 47 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 3d367f1dad..c82d3a5c94 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -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: diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 70c42199a6..8608d3b1e7 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -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; diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 65350c5812..f7f87516c6 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -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: diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index 9ee1311803..f1379c8aa5 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -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), diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 805a80eaed..b8ff3fcc51 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -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, diff --git a/ArduPlane/events.pde b/ArduPlane/events.pde index 082c53261b..2ef2b546f5 100644 --- a/ArduPlane/events.pde +++ b/ArduPlane/events.pde @@ -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); diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde index 61a6823c2c..7d4888c488 100644 --- a/ArduPlane/navigation.pde +++ b/ArduPlane/navigation.pde @@ -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, ¤t_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, diff --git a/ArduPlane/setup.pde b/ArduPlane/setup.pde index c57dfd9ac0..61c9a984c0 100644 --- a/ArduPlane/setup.pde +++ b/ArduPlane/setup.pde @@ -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) diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index 2921674188..064b921944 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -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;