APMrover v2.1.6 - Added Speed Booster between wp and soft start

Signed-off-by: Jean-Louis Naudin <jlnaudin@gmail.com>

APMrover2/APM_Config.h

@@ -20,8 +20,8 @@
 
 #define TRACE               DISABLED
 
-//#include "APM_Config_HILmode.h"  // for test in HIL mode with AeroSIM Rc 3.83
+#include "APM_Config_HILmode.h"  // for test in HIL mode with AeroSIM Rc 3.83
-#include "APM_Config_Rover.h"      // to be used with the real Traxxas model Monster Jam Grinder
+//#include "APM_Config_Rover.h"      // to be used with the real Traxxas model Monster Jam Grinder
 
 // Radio setup:
 // APM INPUT (Rec = receiver)

APMrover2/APM_Config_HILmode.h

@@ -53,6 +53,11 @@
 #define MAX_DIST       50  //300       // max distance (in m) for the HEADALT mode
 #define SARSEC_BRANCH  50              // Long branch of the SARSEC pattern
 
+#define BOOSTER              2    // booster factor x2
+#define AUTO_WP_RADIUS       DISABLED
+#define AIRSPEED_CRUISE      3    // 4m/s
+#define THROTTLE_SLEW_LIMIT  2    // set to 2 for a smooth acceleration by 0.2 step
+
 //////////////////////////////////////////////////////////////////////////////
 //////////////////////////////////////////////////////////////////////////////
 // FLIGHT AND NAVIGATION CONTROL
@@ -65,7 +70,6 @@
 // is 10m/s, which is a conservative value suitable for relatively small,
 // light aircraft.
 //
-#define AIRSPEED_CRUISE     3
 #define GSBOOST             0   // 60 // boost the throttle if ground speed is too low in case of windy conditions // 100
 #define NUDGE_OFFSET	    0     //1603  // nudge_offset to get a good sustained speed in autonomous flight
 #define MIN_GNDSPEED       3
@@ -373,7 +377,6 @@
 #define THROTTLE_TE_I       0.0
 #define THROTTLE_TE_D       0.0
 #define THROTTLE_TE_INT_MAX 20
-#define THROTTLE_SLEW_LIMIT 0
 #define P_TO_T              0.0
 #define T_TO_P              0
 

APMrover2/APM_Config_Rover.h

@@ -33,7 +33,6 @@
 #define GPS_PROTOCOL          GPS_PROTOCOL_AUTO
 
 #define CH7_OPTION	      CH7_SAVE_WP
-#define TUNING_OPTION	      TUN_NONE
 
 #define FLIGHT_MODE_1         AUTO         // pos 0 ---
 #define FLIGHT_MODE_2         AUTO         // pos 1
@@ -49,10 +48,21 @@
 #define TURN_GAIN		5
 
 #define CLOSED_LOOP_NAV       ENABLED     // set to ENABLED if closed loop navigation else set to DISABLED (Return To Lauch)
-#define AUTO_WP_RADIUS        DISABLED
 
 #define MAX_DIST             50  //300       // max distance (in m) for the HEADALT mode
 #define SARSEC_BRANCH        50              // Long branch of the SARSEC pattern
+/*
+During straight lines if the speed booster is enabled, after passing the Wp, 
+the speed is multplied by a speed factor ROV_BOOSTER   =  2     
+(i.e. this is my tested value... so the required speed will be 2 x 4 = 8 m/s in straight lines), 
+the when the rover approach the wp, it slow down to 4 m/s (TRIM_ARSPD_CM)...
+This feature works only if the ROV_AWPR_NAV is set to 0
+*/
+
+#define BOOSTER              2    // booster factor x2
+#define AUTO_WP_RADIUS       DISABLED
+#define AIRSPEED_CRUISE      4    // 4m/s
+#define THROTTLE_SLEW_LIMIT  2    // set to 2 for a smooth acceleration by 0.2 step
 
 //////////////////////////////////////////////////////////////////////////////
 //////////////////////////////////////////////////////////////////////////////
@@ -66,7 +76,6 @@
 // is 10m/s, which is a conservative value suitable for relatively small,
 // light aircraft.
 //
-#define AIRSPEED_CRUISE     3
 #define GSBOOST             0  
 #define NUDGE_OFFSET	    0  
 #define MIN_GNDSPEED       3

APMrover2/APMrover2.pde

@@ -1,6 +1,6 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-#define THISFIRMWARE "APMrover v2.14 JL NAUDIN" //New version of the APMrover for the APM v1 or APM v2 and magnetometer + SONAR
+#define THISFIRMWARE "APMrover v2.16 JL NAUDIN" //New version of the APMrover for the APM v1 or APM v2 and magnetometer + SONAR
 
 // This is a full version of Arduplane v2.32 specially adapted for a Rover by Jean-Louis Naudin (JLN) 
 
@@ -18,7 +18,7 @@ License as published by the Free Software Foundation; either
 version 2.1 of the License, or (at your option) any later version.
 
 //
-// JLN updates: last update 2012-05-15
+// JLN updates: last update 2012-05-17
 //
 // DOLIST:
 //  
@@ -26,6 +26,9 @@ version 2.1 of the License, or (at your option) any later version.
 //-------------------------------------------------------------------------------------------------------------------------
 // Dev Startup : 2012-04-21
 //
+//  2012-05-17: added speed_boost during straight line
+//  2012-05-17: New update about the throttle rate control based on the field test done by Franco Borasio (Thanks Franco..)
+//  2012-05-15: The Throttle rate can be controlled by the THROTTLE_SLEW_LIMIT (the value give the step increase, 1 = 0.1)
 //  2012-05-14: Update about mavlink library (now compatible with the latest version of mavlink)
 //  2012-05-14: Added option (hold roll to full right + SW7 ON/OFF) to init_home during the wp_list reset
 //  2012-05-13: Add ROV_SONAR_TRIG (default = 200 cm)
@@ -505,6 +508,7 @@ static int16_t		sonar_dist;
 // The amount current ground speed is below min ground speed.  Centimeters per second
 static long		groundspeed_undershoot = 0;
 static long		ground_speed = 0;
+static int throttle_last = 0, throttle = 500;
 
 ////////////////////////////////////////////////////////////////////////////////
 // Location Errors
@@ -583,6 +587,7 @@ static bool  final = false;
 // JLN Update
 unsigned long  timesw                  = 0;
 static long 	ground_course = 0;		      // deg * 100 dir of plane
+static bool speed_boost = false;
 
 ////////////////////////////////////////////////////////////////////////////////
 // Loiter management
@@ -1023,9 +1028,7 @@ static void slow_loop()
 
 #if TRACE == ENABLED
               Serial.printf_P(PSTR("NAV->gnd_crs=%3.0f, nav_brg=%3.0f, tgt_brg=%3.0f, brg_err=%3.0f, nav_rll=%3.1f rsvo=%3.1f\n"), 
-                    (float)ground_course/100, (float)nav_bearing/100, (float)target_bearing/100, (float)bearing_error/100, (float)nav_roll/100, (float)g.channel_roll.servo_out/100);           
+                    (float)ground_course/100, (float)nav_bearing/100, (float)target_bearing/100, (float)bearing_error/100, (float)nav_roll/100, (float)g.channel_roll.servo_out/100);                                
-             // Serial.printf_P(PSTR("WPL->g.command_total=%d, g.command_index=%d, nav_command_index=%d\n"), 
-                //    g.command_total, g.command_index, nav_command_index);                                 
 #endif                          
 			break;
 	}
@@ -1165,7 +1168,6 @@ static void update_current_flight_mode(void)
 				g.channel_pitch.servo_out = g.channel_pitch.pwm_to_angle();
 				g.channel_rudder.servo_out = g.channel_roll.pwm_to_angle();
 				break;
-				//roll: -13788.000,  pitch: -13698.000,   thr: 0.000, rud: -13742.000
 
 		}
 	}

APMrover2/Attitude.pde

@@ -25,14 +25,33 @@ static void crash_checker()
 }
 
 static void calc_throttle()
-{           
+{  int rov_speed;
-	int throttle_target = g.throttle_cruise + throttle_nudge + 50;
+    
-
+   int throttle_target = g.throttle_cruise + throttle_nudge + 50;   
-        // Normal airspeed target
+   
-        target_airspeed = g.airspeed_cruise;       
+        target_airspeed = g.airspeed_cruise;    
-        groundspeed_error = target_airspeed - (float)ground_speed;       
+     
-        g.channel_throttle.servo_out = throttle_target + g.pidTeThrottle.get_pid(groundspeed_error, dTnav);        
+   if(speed_boost)
-	g.channel_throttle.servo_out = constrain(g.channel_throttle.servo_out, g.throttle_min.get(), g.throttle_max.get());
+     rov_speed = g.booster * target_airspeed;
+   else 
+     rov_speed = target_airspeed;   
+        
+        groundspeed_error = rov_speed - (float)ground_speed; 
+        
+        int throttle_req = (throttle_target + g.pidTeThrottle.get_pid(groundspeed_error, dTnav)) * 10;
+        
+        if(g.throttle_slewrate > 0)
+        { if (throttle_req > throttle_last) 
+	      throttle = throttle + g.throttle_slewrate;
+          else if (throttle_req  < throttle_last) {
+	      throttle = throttle - g.throttle_slewrate;   
+          }
+    	  throttle =  constrain(throttle, 500, throttle_req);
+          throttle_last = throttle;
+        } else {
+          throttle = throttle_req;
+        }
+        g.channel_throttle.servo_out = constrain(((float)throttle / 10.0f), 0, g.throttle_max.get());
 }
 
 /*****************************************
@@ -74,6 +93,7 @@ float roll_slew_limit(float servo)
  *****************************************/
 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
 
@@ -81,9 +101,9 @@ static void throttle_slew_limit()
 		g.channel_throttle.radio_out = constrain(g.channel_throttle.radio_out, last - (int)temp, last + (int)temp);
 		last = g.channel_throttle.radio_out;
 	}
+*/
 }
 
-
 // Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
 // Keeps outdated data out of our calculations
 static void reset_I(void)
@@ -125,19 +145,12 @@ static void set_servos(void)
 		g.channel_rudder.calc_pwm();             
 
 		g.channel_throttle.radio_out 	= g.channel_throttle.radio_in;
-
-		// 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());
 
-  		 //      g.channel_throttle.calc_pwm();
-
-		/*  TO DO - fix this for RC_Channel library
-		#if THROTTLE_REVERSE == 1
-			radio_out[CH_THROTTLE] = radio_max(CH_THROTTLE) + radio_min(CH_THROTTLE) - radio_out[CH_THROTTLE];
-		#endif
-		*/
         }
+                
         if (control_mode >= FLY_BY_WIRE_B) {
+          // convert 0 to 100% into PWM
             g.channel_throttle.calc_pwm();
             /* only do throttle slew limiting in modes where throttle
                control is automatic */

APMrover2/Parameters.h

@@ -133,6 +133,7 @@ public:
         k_param_auto_wp_radius,
         k_param_sonar_trigger,
         k_param_turn_gain,
+        k_param_booster,        
         
 // ************************************************************
 //
@@ -351,7 +352,8 @@ public:
         AP_Int8     auto_wp_radius;
         AP_Int16    sonar_trigger;
         AP_Int16    turn_gain;
-    
+        AP_Int8     booster;
+        
 // ************************************************************   
 
         // RC channels
@@ -482,7 +484,8 @@ public:
         closed_loop_nav         (CLOSED_LOOP_NAV),
         auto_wp_radius          (AUTO_WP_RADIUS), 
         sonar_trigger           (SONAR_TRIGGER),
-        turn_gain               (TURN_GAIN), 
+        turn_gain               (TURN_GAIN),
+        booster                 (BOOSTER),        
 
 // ************************************************************   
         

APMrover2/Parameters.pde

@@ -124,6 +124,8 @@ static const AP_Param::Info var_info[] PROGMEM = {
         GSCALAR(auto_wp_radius,         "ROV_AWPR_NAV"),
         GSCALAR(sonar_trigger,          "ROV_SONAR_TRIG"),
         GSCALAR(turn_gain,              "ROV_GAIN"),
+        GSCALAR(booster,                 "ROV_BOOSTER"),
+        
 // ************************************************************
 
 	GGROUP(channel_roll,            "RC1_", RC_Channel),

APMrover2/system.pde

@@ -351,7 +351,9 @@ static void set_mode(byte mode)
 
 	control_mode = mode;
 	crash_timer = 0;
-
+        throttle_last = 0;
+        throttle = 500;
+        
 	switch(control_mode)
 	{
 		case MANUAL: