ACM Acceleration management

removed unused waypoint_radius var
removed slow_wp
updated speed management system to maintain a constant acceleration or deceleration of 1m/s
changed version to 2.8.1a

ArduCopter/ArduCopter.pde

@@ -1,6 +1,6 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-#define THISFIRMWARE "ArduCopter V2.8"
+#define THISFIRMWARE "ArduCopter V2.8.1a"
 /*
  *  ArduCopter Version 2.8
  *  Lead author:	Jason Short
@@ -557,13 +557,12 @@ static uint8_t command_cond_index;
 // NAV_DELAY    - have we waited at the waypoint the desired time?
 static uint8_t wp_verify_byte;                                                  // used for tracking state of navigating waypoints
 // used to limit the speed ramp up of WP navigation
-// Acceleration is limited to .5m/s/s
+// Acceleration is limited to 1m/s/s
-static int16_t waypoint_speed_gov;
+static int16_t max_speed_old;
 // Used to track how many cm we are from the "next_WP" location
 static int32_t long_error, lat_error;
 // Are we navigating while holding a positon? This is set to false once the speed drops below 1m/s
 static boolean loiter_override;
-static int16_t waypoint_radius;
 
 static int16_t control_roll;
 static int16_t control_pitch;
@@ -795,7 +794,6 @@ static int16_t nav_lon;
 static int32_t of_roll;
 // The Commanded pitch from the autopilot based on optical flow sensor. negative Pitch means go forward.
 static int32_t of_pitch;
-static bool slow_wp = false;
 
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -2072,8 +2070,6 @@ static void update_nav_RTL()
         else
             loiter_timer = 0;
     }
-
-    slow_wp = true;
 }
 
 static void read_AHRS(void)
@@ -2434,7 +2430,7 @@ static void update_nav_wp()
         // nav_lon, nav_lat is calculated
 
         if(wp_distance > 400) {
-            calc_nav_rate(get_desired_speed(g.waypoint_speed_max, true));
+            calc_nav_rate(get_desired_speed(g.waypoint_speed_max));
         }else{
             // calc the lat and long error to the target
             calc_location_error(&next_WP);
@@ -2454,7 +2450,7 @@ static void update_nav_wp()
         // calc error to target
         calc_location_error(&next_WP);
 
-        int16_t speed = get_desired_speed(g.waypoint_speed_max, slow_wp);
+        int16_t speed = get_desired_speed(g.waypoint_speed_max);
         // use error as the desired rate towards the target
         calc_nav_rate(speed);
 

ArduCopter/Attitude.pde

@@ -536,14 +536,8 @@ static void reset_nav_params(void)
     long_error                              = 0;
     lat_error                               = 0;
 
-    // We want to by default pass WPs
-    slow_wp = false;
-
     // make sure we stick to Nav yaw on takeoff
     auto_yaw = nav_yaw;
-
-    // revert to smaller radius set in params
-    waypoint_radius = g.waypoint_radius;
 }
 
 /*

ArduCopter/Parameters.pde

@@ -402,7 +402,7 @@ static void load_parameters(void)
     // change the default for the AHRS_GPS_GAIN for ArduCopter
     // if it hasn't been set by the user
     if (!ahrs.gps_gain.load()) {
-        ahrs.gps_gain.set_and_save(0.0);
+        ahrs.gps_gain.set_and_save(1.0);
     }
 
     // setup different AHRS gains for ArduCopter than the default

ArduCopter/commands.pde

@@ -10,6 +10,7 @@ static void init_commands()
     command_nav_queue.id    = NO_COMMAND;
 
     fast_corner                     = false;
+	reset_desired_speed();
 
     // default Yaw tracking
     yaw_tracking                    = MAV_ROI_WPNEXT;
@@ -176,10 +177,6 @@ static void set_next_WP(struct Location *wp)
     // to check if we have missed the WP
     // ---------------------------------
     original_target_bearing = target_bearing;
-
-    // reset speed governer
-    // --------------------
-    waypoint_speed_gov = g_gps->ground_speed;
 }
 
 

ArduCopter/commands_logic.pde

@@ -227,10 +227,6 @@ static void do_RTL(void)
     // --------------------
     set_next_WP(&temp);
 
-
-    // We want to come home and stop on a dime
-    slow_wp = true;
-
     // output control mode to the ground station
     // -----------------------------------------
     gcs_send_message(MSG_HEARTBEAT);
@@ -260,7 +256,6 @@ static void do_takeoff()
 static void do_nav_wp()
 {
     wp_control = WP_MODE;
-    slow_wp = false;
 
     set_next_WP(&command_nav_queue);
 
@@ -449,7 +444,7 @@ static bool verify_nav_wp()
     }
 
     // Did we pass the WP?	// Distance checking
-    if((wp_distance <= (waypoint_radius * 100)) || check_missed_wp()) {
+    if((wp_distance <= (g.waypoint_radius * 100)) || check_missed_wp()) {
 
         // if we have a distance calc error, wp_distance may be less than 0
         if(wp_distance > 0) {

ArduCopter/commands_process.pde

@@ -83,7 +83,14 @@ static void update_commands()
                     temp = wrap_180(temp);
                     fast_corner = labs(temp) < 6000;
                 }
+
+                // If we try and stop at a corner, lets reset our desired speed to prevent
+                // too much jerkyness.
+				if(false == fast_corner){
+					reset_desired_speed();
+				}
             }
+
         }else{
             // we are out of commands
             exit_mission();

ArduCopter/navigation.pde

@@ -239,36 +239,30 @@ static void calc_loiter_pitch_roll()
     auto_pitch = -auto_pitch;
 }
 
-static int16_t get_desired_speed(int16_t max_speed, bool _slow)
+static int16_t get_desired_speed(int16_t max_speed)
 {
-    /*
-     * |< WP Radius
-     *  0  1   2   3   4   5   6   7   8m
-     *  ...|...|...|...|...|...|...|...|
-     *         100  |  200	  300	  400cm/s
-     |                              +|+
-     ||< we should slow to 1.5 m/s as we hit the target
-     */
-
     if(fast_corner) {
-        waypoint_radius = g.waypoint_radius * 2;
+        // don't slow down
-        //max_speed         = max_speed;
     }else{
-        waypoint_radius = g.waypoint_radius;
+        if(wp_distance < 15000){
-        max_speed               = min(max_speed, (wp_distance - g.waypoint_radius) / 3);
+            // go slower
-        max_speed               = max(max_speed, WAYPOINT_SPEED_MIN);           // go at least 100cm/s
+    	 	int32_t temp = 2 * 100 * (wp_distance - g.waypoint_radius * 100);
+    		max_speed = sqrt((float)temp);
+            max_speed = min(max_speed, g.waypoint_speed_max);
         }
-
-    // limit the ramp up of the speed
-    // waypoint_speed_gov is reset to 0 at each new WP command
-    if(max_speed > waypoint_speed_gov) {
-        waypoint_speed_gov += (int)(100.0 * dTnav);         // increase at .5/ms
-        max_speed = waypoint_speed_gov;
     }
 
+    max_speed 		= min(max_speed, max_speed_old + (100 * dTnav));// limit going faster
+    max_speed 		= max(max_speed, WAYPOINT_SPEED_MIN); 	// don't go too slow
+    max_speed_old 	= max_speed;
     return max_speed;
 }
 
+static int16_t reset_desired_speed()
+{
+    max_speed_old = 0;
+}
+
 static int16_t get_desired_climb_rate()
 {
     if(alt_change_flag == ASCENDING) {

ArduCopter/system.pde

@@ -424,9 +424,6 @@ static void set_mode(byte mode)
     // clearing value used to force the copter down in landing mode
     landing_boost = 0;
 
-    // do we want to come to a stop or pass a WP?
-    slow_wp = false;
-
     // do not auto_land if we are leaving RTL
     loiter_timer = 0;
 

ArduCopter/test.pde

@@ -184,6 +184,24 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv)
  *  }*/
 
 
+/*
+//static int8_t
+//test_toy(uint8_t argc, const Menu::arg *argv)
+{
+	wp_distance = 0;
+	int16_t max_speed = 0;
+
+ 	for(int16_t i = 0; i < 200; i++){
+	 	int32_t temp = 2 * 100 * (wp_distance - g.waypoint_radius * 100);
+		max_speed = sqrt((float)temp);
+		max_speed = min(max_speed, g.waypoint_speed_max);
+		Serial.printf("Zspeed: %ld, %d, %ld\n", temp, max_speed, wp_distance);
+	 	wp_distance += 100;
+	}
+ 	return 0;
+}
+//*/
+
 /*static int8_t
  *  //test_toy(uint8_t argc, const Menu::arg *argv)
  *  {