Rover: added filter to auto steering, and fixed throttle pid

APMrover2/APMrover2.pde

@@ -91,6 +91,7 @@ version 2.1 of the License, or (at your option) any later version.
 #include <RC_Channel.h>     // RC Channel Library
 #include <AP_RangeFinder.h>	// Range finder library
 #include <Filter.h>			// Filter library
+#include <Butter.h>			// Filter library - butterworth filter
 #include <AP_Buffer.h>      // FIFO buffer library
 #include <ModeFilter.h>		// Mode Filter from Filter library
 #include <AverageFilter.h>	// Mode Filter from Filter library
@@ -416,8 +417,8 @@ static int16_t throttle_last = 0, throttle = 500;
 ////////////////////////////////////////////////////////////////////////////////
 // Location Errors
 ////////////////////////////////////////////////////////////////////////////////
-// Difference between current bearing and desired bearing.  Hundredths of a degree
+// Difference between current bearing and desired bearing.  in centi-degrees
-static int32_t bearing_error;
+static int32_t bearing_error_cd;
 // Difference between current altitude and desired altitude.  Centimeters
 static int32_t altitude_error;
 // Distance perpandicular to the course line that we are off trackline.  Meters 

APMrover2/Steering.pde

@@ -23,7 +23,7 @@ static void calc_throttle()
    
    groundspeed_error = g.speed_cruise - ground_speed; 
         
-   throttle = throttle_target + g.pidSpeedThrottle.get_pid(groundspeed_error);
+   throttle = throttle_target + (g.pidSpeedThrottle.get_pid(groundspeed_error * 100) / 100);
    g.channel_throttle.servo_out = constrain_int16(throttle, g.throttle_min.get(), g.throttle_max.get());
 }
 
@@ -42,22 +42,13 @@ static void calc_nav_steer()
 
     // negative error = left turn
 	// positive error = right turn
-
+	nav_steer = g.pidNavSteer.get_pid(bearing_error_cd, nav_gain_scaler);
-	nav_steer = g.pidNavSteer.get_pid(bearing_error, nav_gain_scaler);	//returns desired bank angle in degrees*100
 
     if (obstacle) {  // obstacle avoidance 
 	    nav_steer += 9000;    // if obstacle in front turn 90° right	
     }
 }
 
-// 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)
-{
-	g.pidNavSteer.reset_I();
-	g.pidSpeedThrottle.reset_I();
-}
-
 /*****************************************
 * Set the flight control servos based on the current calculated values
 *****************************************/

APMrover2/commands.pde

@@ -185,7 +185,8 @@ void init_home()
 
 static void restart_nav()
 {  
-    reset_I();
+    g.pidNavSteer.reset_I();
+    g.pidSpeedThrottle.reset_I();
     prev_WP = current_loc;
     nav_command_ID = NO_COMMAND;
     nav_command_index = 0;

APMrover2/commands_logic.pde

@@ -8,11 +8,11 @@ handle_process_nav_cmd()
 {
 	// reset navigation integrators
 	// -------------------------
-	reset_I();
+    g.pidNavSteer.reset_I();
+
+    gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nav_command.id);
 
-		gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nav_command.id);
 	switch(next_nav_command.id){
-
 		case MAV_CMD_NAV_TAKEOFF:
 			do_takeoff();
 			break;

APMrover2/control_modes.pde

@@ -24,10 +24,10 @@ static void read_control_switch()
 		oldSwitchPosition = switchPosition;
 		prev_WP = current_loc;
 
-		// reset navigation integrators
+		// reset navigation and speed integrators
 		// -------------------------
-		reset_I();
+        g.pidNavSteer.reset_I();
-
+        g.pidSpeedThrottle.reset_I();
 	}
 
 }

APMrover2/navigation.pde

@@ -40,8 +40,10 @@ static void navigate()
 
 static void calc_bearing_error()
 {    
-	bearing_error = nav_bearing - ahrs.yaw_sensor;
+    static butter10hz1_6 butter;
-	bearing_error = wrap_180(bearing_error);
+
+	bearing_error_cd = wrap_180(nav_bearing - ahrs.yaw_sensor);
+    bearing_error_cd = butter.filter(bearing_error_cd);
 }
 
 static long wrap_360(long error)