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

@@ -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;
-
-            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;
+        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;
+            }                 
+
+            if (control_mode != CRUISE || !cruise_state.locked_heading) {
+                nav_roll_cd = channel_roll->norm_input() * g.roll_limit_cd;
+            } else {
+                calc_nav_roll();
             }
-            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:

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;

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:

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),

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,

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);

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, &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,

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)

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;