Minimum ground speed patch from Claudio Natoli. Thanks Claudio!

This patch will boost the target airspeed as necessary to keep the ground speed above a parameter value - param_min_groundspeed.  Airspeed is still limited to FBW-max.  Setting min_groundspeed to zero (default) disables the feature.

ArduPlane/APM_Config.h.reference

@@ -516,6 +516,16 @@
 //#define AIRSPEED_CRUISE     12
 //
 
+//////////////////////////////////////////////////////////////////////////////
+// MIN_GNDSPEED                             OPTIONAL
+//
+// The minimum ground speed in metres per second to maintain during
+// cruise. A value of 0 will disable any attempt to maintain a minumum
+// speed over ground.
+//
+#define MIN_GNDSPEED	0
+//
+
 //////////////////////////////////////////////////////////////////////////////
 // FLY_BY_WIRE_B airspeed control (also used for throttle "nudging" in AUTO)
 //

ArduPlane/ArduPlane.pde

@@ -325,10 +325,14 @@ static byte	non_nav_command_ID	= NO_COMMAND;	// active non-nav command ID
 // --------
 static int		airspeed;							// m/s * 100
 static int		airspeed_nudge;  					// m/s * 100 : additional airspeed based on throttle stick position in top 1/2 of range
+static long		target_airspeed;					// m/s * 100 (used for Auto-flap deployment in FBW_B mode)
 static float	airspeed_error;						// m/s * 100
-static float	airspeed_fbwB;						// m/s * 100
 static long 	energy_error;                       // energy state error (kinetic + potential) for altitude hold
-static long		airspeed_energy_error;              // kinetic portion of energy error
+static long		airspeed_energy_error;              // kinetic portion of energy error (m^2/s^2)
+
+// Ground speed
+static long		groundspeed_undershoot = 0;				// m/s * 100  (>=0, where > 0 => amount below min ground speed)
+
 
 // Location Errors
 // ---------------
@@ -523,21 +527,21 @@ static void fast_loop()
 	read_radio();
 
     // try to send any deferred messages if the serial port now has
-    // some space available    
+    // some space available
     gcs_send_message(MSG_RETRY_DEFERRED);
 
 	// check for loss of control signal failsafe condition
 	// ------------------------------------
 	check_short_failsafe();
-	
+
-		// Read Airspeed
+	// Read Airspeed
-		// -------------
+	// -------------
 	if (g.airspeed_enabled == true) {
 #if HIL_MODE != HIL_MODE_ATTITUDE
 		read_airspeed();
-#endif
+#else
-	} else if (g.airspeed_enabled == true && HIL_MODE == HIL_MODE_ATTITUDE) {
 		calc_airspeed_errors();
+#endif
 	}
 
 	#if HIL_MODE == HIL_MODE_SENSORS
@@ -599,7 +603,10 @@ static void medium_loop()
 		//-------------------------------
 		case 0:
 			medium_loopCounter++;
-			if(GPS_enabled)		update_GPS();
+			if(GPS_enabled){
+                update_GPS();
+                calc_gndspeed_undershoot();
+            }
 
 			#if HIL_MODE != HIL_MODE_ATTITUDE
 				if(g.compass_enabled){
@@ -888,23 +895,10 @@ static void update_current_flight_mode(void)
 				if ((current_loc.alt>=home.alt+g.FBWB_min_altitude) || (g.FBWB_min_altitude == -1)) {
 	 				altitude_error = g.channel_pitch.norm_input() * g.pitch_limit_min;
 				} else {
-					if (g.channel_pitch.norm_input()<0) 
+					if (g.channel_pitch.norm_input()<0)
-						altitude_error =( (home.alt + g.FBWB_min_altitude) - current_loc.alt) + g.channel_pitch.norm_input() * g.pitch_limit_min ; 
+						altitude_error =( (home.alt + g.FBWB_min_altitude) - current_loc.alt) + g.channel_pitch.norm_input() * g.pitch_limit_min ;
-					else altitude_error =( (home.alt + g.FBWB_min_altitude) - current_loc.alt) ;                                    
+					else altitude_error =( (home.alt + g.FBWB_min_altitude) - current_loc.alt) ;
 				}
- 
-				if (g.airspeed_enabled == true)
-									{
-					airspeed_fbwB = ((int)(g.flybywire_airspeed_max -
-							g.flybywire_airspeed_min) *
-							g.channel_throttle.servo_out) +
-							((int)g.flybywire_airspeed_min * 100);
-					airspeed_energy_error = (long)(((long)airspeed_fbwB *
-								(long)airspeed_fbwB) -
-							((long)airspeed * (long)airspeed))/20000;
-					airspeed_error = (airspeed_error - airspeed);
-									}
-
 				calc_throttle();
 				calc_nav_pitch();
 				break;

ArduPlane/Attitude.pde

@@ -127,6 +127,11 @@ static void calc_throttle()
   if (g.airspeed_enabled == false) {
 	int throttle_target = g.throttle_cruise + throttle_nudge;
 
+    // TODO: think up an elegant way to bump throttle when
+    // groundspeed_undershoot > 0 in the no airspeed sensor case; PID
+    // control?
+
+
 		// no airspeed sensor, we use nav pitch to determine the proper throttle output
 		// AUTO, RTL, etc
 		// ---------------------------------------------------------------------------
@@ -236,7 +241,7 @@ static void throttle_slew_limit()
 {
 	static int last = 1000;
 	if(g.throttle_slewrate) {		// if slew limit rate is set to zero then do not slew limit
-	
+
 		float temp = g.throttle_slewrate * G_Dt * 10.f;		//  * 10 to scale % to pwm range of 1000 to 2000
 Serial.print("radio ");	Serial.print(g.channel_throttle.radio_out); Serial.print("   temp "); Serial.print(temp); Serial.print("   last "); Serial.println(last);
 		g.channel_throttle.radio_out = constrain(g.channel_throttle.radio_out, last - (int)temp, last + (int)temp);
@@ -262,7 +267,7 @@ static void reset_I(void)
 static void set_servos(void)
 {
 	int flapSpeedSource = 0;
-	
+
 	// vectorize the rc channels
 	RC_Channel_aux* rc_array[NUM_CHANNELS];
 	rc_array[CH_1] = NULL;
@@ -325,7 +330,7 @@ static void set_servos(void)
 		#else
 			// convert 0 to 100% into PWM
 			g.channel_throttle.servo_out = constrain(g.channel_throttle.servo_out, g.throttle_min.get(), g.throttle_max.get());
-			
+
 			// We want to supress the throttle if we think we are on the ground and in an autopilot controlled throttle mode.
 			/* Disable throttle if following conditions are met:
 				1 - We are in Circle mode (which we use for short term failsafe), or in FBW-B or higher
@@ -336,7 +341,7 @@ static void set_servos(void)
 				OR
 				5 - Home location is not set
 			*/
-			if ( 
+			if (
 					(control_mode == CIRCLE || control_mode >= FLY_BY_WIRE_B) &&
 					(abs(home.alt - current_loc.alt) < 1000) &&
 					((g.airspeed_enabled ? airspeed : g_gps->ground_speed) < 500 ) &&
@@ -345,7 +350,7 @@ static void set_servos(void)
 				g.channel_throttle.servo_out = 0;
 				g.channel_throttle.calc_pwm();
 			}
-			
+
 		#endif
 
 		g.channel_throttle.calc_pwm();
@@ -369,8 +374,9 @@ static void set_servos(void)
 				g_rc_function[RC_Channel_aux::k_flap_auto]->radio_out = g_rc_function[RC_Channel_aux::k_flap_auto]->radio_trim;
 			}
 		} else if (control_mode >= FLY_BY_WIRE_B) {
+            // FIXME: use target_airspeed in both FBW_B and g.airspeed_enabled cases - Doug?
 			if (control_mode == FLY_BY_WIRE_B) {
-				flapSpeedSource = airspeed_fbwB/100;
+				flapSpeedSource = ((float)target_airspeed)/100;
 			} else if (g.airspeed_enabled == true) {
 				flapSpeedSource = g.airspeed_cruise/100;
 			} else {
@@ -417,4 +423,3 @@ static void demo_servos(byte i) {
 		i--;
 	}
 }
-

ArduPlane/Parameters.h

@@ -116,6 +116,7 @@ public:
         k_param_airspeed_cruise,
         k_param_RTL_altitude,
         k_param_inverted_flight_ch,
+        k_param_min_gndspeed,
 
         //
         // 170: Radio settings
@@ -308,6 +309,7 @@ public:
     AP_Int16    log_bitmask;
     AP_Int16	log_last_filenumber;
     AP_Int16    airspeed_cruise;
+    AP_Int16    min_gndspeed;
     AP_Int16    pitch_trim;
     AP_Int16    RTL_altitude;
     AP_Int16    ground_temperature;
@@ -414,6 +416,7 @@ public:
         log_bitmask             (DEFAULT_LOG_BITMASK,		k_param_log_bitmask,            PSTR("LOG_BITMASK")),
         log_last_filenumber     (0,							k_param_log_last_filenumber,    PSTR("LOG_LASTFILE")),
         airspeed_cruise         (AIRSPEED_CRUISE_CM,        k_param_airspeed_cruise,        PSTR("TRIM_ARSPD_CM")),
+        min_gndspeed            (MIN_GNDSPEED_CM,        	k_param_min_gndspeed,           PSTR("MIN_GNDSPD_CM")),
         pitch_trim              (0,                         k_param_pitch_trim,             PSTR("TRIM_PITCH_CD")),
         RTL_altitude            (ALT_HOLD_HOME_CM,          k_param_RTL_altitude,           PSTR("ALT_HOLD_RTL")),
         FBWB_min_altitude       (ALT_HOLD_FBW_CM,           k_param_FBWB_min_altitude,      PSTR("ALT_HOLD_FBWCM")),

ArduPlane/commands_logic.pde

@@ -498,9 +498,16 @@ static void do_jump()
 
 static void do_change_speed()
 {
-	// Note: we have no implementation for commanded ground speed, only air speed and throttle
+	switch (next_nonnav_command.p1)
-	if(next_nonnav_command.alt > 0)
+	{
-		g.airspeed_cruise.set_and_save(next_nonnav_command.alt * 100);
+		case 0: // Airspeed
+			if(next_nonnav_command.alt > 0)
+				g.airspeed_cruise.set_and_save(next_nonnav_command.alt * 100);
+			break;
+		case 1: // Ground speed
+			g.min_gndspeed.set_and_save(next_nonnav_command.alt * 100);
+			break;
+	}
 
 	if(next_nonnav_command.lat > 0)
 		g.throttle_cruise.set_and_save(next_nonnav_command.lat);

ArduPlane/config.h

@@ -456,6 +456,16 @@
 #endif
 #define AIRSPEED_CRUISE_CM AIRSPEED_CRUISE*100
 
+
+//////////////////////////////////////////////////////////////////////////////
+// MIN_GNDSPEED
+//
+#ifndef MIN_GNDSPEED
+# define MIN_GNDSPEED			0 // m/s (0 disables)
+#endif
+#define MIN_GNDSPEED_CM MIN_GNDSPEED*100
+
+
 //////////////////////////////////////////////////////////////////////////////
 // FLY_BY_WIRE_B airspeed control
 //

ArduPlane/navigation.pde

@@ -64,14 +64,43 @@ void calc_distance_error()
 
 static void calc_airspeed_errors()
 {
-    // XXX excess casting here
+    // Normal airspeed target
-	if(control_mode>=AUTO && airspeed_nudge > 0) {
+    target_airspeed = g.airspeed_cruise;
-		airspeed_error = g.airspeed_cruise + airspeed_nudge - airspeed;
+
-		airspeed_energy_error = (long)(((long)(g.airspeed_cruise + airspeed_nudge) * (long)(g.airspeed_cruise + airspeed_nudge)) - ((long)airspeed * (long)airspeed))/20000; //Changed 0.00005f * to / 20000 to avoid floating point calculation
+    // FBW_B airspeed target
-	} else {
+    if (control_mode == FLY_BY_WIRE_B) {
-		airspeed_error = g.airspeed_cruise - airspeed;
+        target_airspeed = ((int)(g.flybywire_airspeed_max -
-		airspeed_energy_error = (long)(((long)g.airspeed_cruise * (long)g.airspeed_cruise) - ((long)airspeed * (long)airspeed))/20000; //Changed 0.00005f * to / 20000 to avoid floating point calculation
+                                 g.flybywire_airspeed_min) *
-	}
+                           g.channel_throttle.servo_out) +
+                          ((int)g.flybywire_airspeed_min * 100);
+    }
+
+    // Set target to current airspeed + ground speed undershoot,
+    // but only when this is faster than the target airspeed commanded
+    // above.
+    if (control_mode >= FLY_BY_WIRE_B && (g.min_gndspeed > 0)) {
+        long min_gnd_target_airspeed = airspeed + groundspeed_undershoot;
+        if (min_gnd_target_airspeed > target_airspeed)
+            target_airspeed = min_gnd_target_airspeed;
+    }
+
+    // Bump up the target airspeed based on throttle nudging
+    if (control_mode >= AUTO && airspeed_nudge > 0) {
+        target_airspeed += airspeed_nudge;
+    }
+
+    // Apply airspeed limit
+    if (target_airspeed > (g.flybywire_airspeed_max * 100))
+        target_airspeed = (g.flybywire_airspeed_max * 100);
+
+    airspeed_error = target_airspeed - airspeed;
+    airspeed_energy_error = ((target_airspeed * target_airspeed) - ((long)airspeed * (long)airspeed))/20000; //Changed 0.00005f * to / 20000 to avoid floating point calculation
+}
+
+static void calc_gndspeed_undershoot()
+{
+    // Function is overkill, but here in case we want to add filtering later
+    groundspeed_undershoot = (g.min_gndspeed > 0) ? (g.min_gndspeed - g_gps->ground_speed) : 0;
 }
 
 static void calc_bearing_error()
@@ -156,7 +185,7 @@ static void update_loiter()
 	}else{
 		update_crosstrack();
 		loiter_time = millis();			// keep start time for loiter updating till we get within LOITER_RANGE of orbit
-		
+
 	}
 /*
 	if (wp_distance < g.loiter_radius){

Tools/ArdupilotMegaPlanner/bin/Release/Resources/MAVParam.txt

@@ -102,7 +102,8 @@ It includes both fixed wing (APM) and rotary wing (!ArduCopter) parameters. Some
 ||LOG_BITMASK||0||65535||334||1||1||LOG_BITMASK||
 ||TRIM_ELEVON||900||2100||1500||1||1||TRIM_ELEVON||
 ||THR_FS_VALUE||850||1000||950||1||1||THROTTLE_FS_VALUE - If the throttle failsafe is enabled, THROTTLE_FS_VALUE sets the channel value below which the failsafe engages.  The default is 975ms, which is a very low throttle setting.  Most transmitters will let you trim the manual throttle position up so that you cannot engage the failsafe with a regular stick movement. Configure your receiver's failsafe setting for the throttle channel to the absolute minimum, and use the !ArduPilotMega_demo program to check that you cannot reach that value with the throttle control.  Leave a margin of at least 50 microseconds between the lowest throttle setting and THROTTLE_FS_VALUE.||
-||TRIM_ARSPD_CM||500||5000||1200||100||1||AIRSPEED_CRUISE_CM - The speed in metres per second to maintain during cruise.  The default is 10m/s, which is a conservative value suitable for relatively small, light aircraft.||
+||TRIM_ARSPD_CM||500||5000||1200||100||1||AIRSPEED_CRUISE_CM - The speed in cm/s to maintain during cruise.  The default is 12m/s, which is a conservative value suitable for relatively small, light aircraft.||
+||MIN_GNDSPD_CM||0||5000||0||100||1||MIN_GNDSPEED_CM - The minimum ground in cm/s to maintain during cruise. The default value of 0 will disable any attempt to maintain a minimum speed over ground.||
 ||GND_TEMP||-10||50||28||1||1||GND_TEMP - Ground Temperature||
 ||AP_OFFSET|| || ||0|| || ||AP_OFFSET||
 ||TRIM_PITCH_CD|| || ||0|| || ||TRIM_PITCH_CD||