APM: changed PID library to do automatic deltat calculation

this fixes a problem with the HDNG2RLL PID, which was using the wrong
time base and prevents similar bugs from happening in the future

ArduPlane/ArduPlane.pde

@@ -622,10 +622,6 @@ static byte 			superslow_loopCounter;
 // Counter to trigger execution of 1 Hz processes
 static byte				counter_one_herz;
 
-// used to track the elapsed time for navigation PID integral terms
-static uint32_t 	nav_loopTimer;
-// Elapsed time since last call to navigation pid functions
-static uint32_t 	dTnav;
 // % MCU cycles used
 static float 			load;
 
@@ -815,9 +811,6 @@ Serial.println(tempaccel.z, DEC);
 
 			if(g_gps->new_data){
 				g_gps->new_data 	= false;
-				dTnav 				= millis() - nav_loopTimer;
-				nav_loopTimer 		= millis();
-
 				// calculate the plane's desired bearing
 				// -------------------------------------
 				navigate();

ArduPlane/Attitude.pde

@@ -48,7 +48,7 @@ static void stabilize()
 
 	// Calculate dersired servo output for the roll
 	// ---------------------------------------------
-	g.channel_roll.servo_out = g.pidServoRoll.get_pid((nav_roll - ahrs.roll_sensor), delta_ms_fast_loop, speed_scaler);
+	g.channel_roll.servo_out = g.pidServoRoll.get_pid((nav_roll - ahrs.roll_sensor), speed_scaler);
 	long tempcalc = nav_pitch +
 	        fabs(ahrs.roll_sensor * g.kff_pitch_compensation) +
 	        (g.channel_throttle.servo_out * g.kff_throttle_to_pitch) -
@@ -57,7 +57,7 @@ static void stabilize()
         // when flying upside down the elevator control is inverted
         tempcalc = -tempcalc;
     }
-	g.channel_pitch.servo_out = g.pidServoPitch.get_pid(tempcalc, delta_ms_fast_loop, speed_scaler);
+	g.channel_pitch.servo_out = g.pidServoPitch.get_pid(tempcalc, speed_scaler);
 
 	// Mix Stick input to allow users to override control surfaces
 	// -----------------------------------------------------------
@@ -154,7 +154,7 @@ static void calc_throttle()
 		energy_error = airspeed_energy_error + (float)altitude_error * 0.098f;
 
 		// positive energy errors make the throttle go higher
-		g.channel_throttle.servo_out = g.throttle_cruise + g.pidTeThrottle.get_pid(energy_error, dTnav);
+		g.channel_throttle.servo_out = g.throttle_cruise + g.pidTeThrottle.get_pid(energy_error);
 		g.channel_throttle.servo_out += (g.channel_pitch.servo_out * g.kff_pitch_to_throttle);
 
 		g.channel_throttle.servo_out = constrain(g.channel_throttle.servo_out,
@@ -176,7 +176,7 @@ static void calc_nav_yaw(float speed_scaler)
 	long error = (long)(-temp.y*100.0);
 
 	// Control is a feedforward from the aileron control + a PID to coordinate the turn (drive y axis accel to zero)
-	g.channel_rudder.servo_out = g.kff_rudder_mix * g.channel_roll.servo_out + g.pidServoRudder.get_pid(error, delta_ms_fast_loop, speed_scaler);
+	g.channel_rudder.servo_out = g.kff_rudder_mix * g.channel_roll.servo_out + g.pidServoRudder.get_pid(error, speed_scaler);
 #else
 	g.channel_rudder.servo_out = g.kff_rudder_mix * g.channel_roll.servo_out;
 	// XXX probably need something here based on heading
@@ -189,9 +189,9 @@ static void calc_nav_pitch()
 	// Calculate the Pitch of the plane
 	// --------------------------------
 	if (g.airspeed_enabled == true && g.airspeed_use == true) {
-		nav_pitch = -g.pidNavPitchAirspeed.get_pid(airspeed_error, dTnav);
+		nav_pitch = -g.pidNavPitchAirspeed.get_pid(airspeed_error);
 	} else {
-		nav_pitch = g.pidNavPitchAltitude.get_pid(altitude_error, dTnav);
+		nav_pitch = g.pidNavPitchAltitude.get_pid(altitude_error);
     }
 	nav_pitch = constrain(nav_pitch, g.pitch_limit_min.get(), g.pitch_limit_max.get());
 }
@@ -211,7 +211,7 @@ static void calc_nav_roll()
 	// positive error = right turn
 	// Calculate the required roll of the plane
 	// ----------------------------------------
-	nav_roll = g.pidNavRoll.get_pid(bearing_error, dTnav, nav_gain_scaler);	//returns desired bank angle in degrees*100
+	nav_roll = g.pidNavRoll.get_pid(bearing_error, nav_gain_scaler);	//returns desired bank angle in degrees*100
 	nav_roll = constrain(nav_roll, -g.roll_limit.get(), g.roll_limit.get());
 
 	Vector3f omega;

ArduPlane/config.h

@@ -650,10 +650,10 @@
 # define NAV_ROLL_P           0.7
 #endif
 #ifndef NAV_ROLL_I
-# define NAV_ROLL_I           0.1
+# define NAV_ROLL_I           0.02
 #endif
 #ifndef NAV_ROLL_D
-# define NAV_ROLL_D           0.02
+# define NAV_ROLL_D           0.1
 #endif
 #ifndef NAV_ROLL_INT_MAX
 # define NAV_ROLL_INT_MAX     5

libraries/PID/PID.cpp

@@ -15,11 +15,20 @@ const AP_Param::GroupInfo PID::var_info[] PROGMEM = {
 	AP_GROUPEND
 };
 
-long
+int32_t
-PID::get_pid(int32_t error, uint16_t dt, float scaler)
+PID::get_pid(int32_t error, float scaler)
 {
+	uint32_t tnow = millis();
+	uint32_t dt = tnow - _last_t;
 	float output		= 0;
- 	float delta_time	= (float)dt / 1000.0;
+ 	float delta_time;
+
+	if (_last_t == 0 || dt > 1000) {
+		dt = 0;
+	}
+	_last_t = tnow;
+
+	delta_time = (float)dt / 1000.0;
 
 	// Compute proportional component
 	output += error * _kp;

libraries/PID/PID.h

@@ -38,7 +38,7 @@ public:
 	///
 	/// @returns		The updated control output.
 	///
-	long 	get_pid(int32_t error, uint16_t dt, float scaler = 1.0);
+	int32_t	get_pid(int32_t error, float scaler = 1.0);
 
 	/// Reset the PID integrator
 	///
@@ -86,6 +86,9 @@ private:
 	float				_integrator;		///< integrator value
 	int32_t				_last_error;		///< last error for derivative
 	float				_last_derivative; 	///< last derivative for low-pass filter
+	uint32_t			_last_t;
+
+	int32_t	_get_pid(int32_t error, uint16_t dt, float scaler);
 
 	/// Low pass filter cut frequency for derivative calculation.
 	///

libraries/PID/examples/pid/pid.pde

@@ -6,6 +6,7 @@
 #include <FastSerial.h>
 #include <avr/pgmspace.h>
 #include <AP_Common.h>
+#include <AP_Math.h>
 #include <APM_RC.h> // ArduPilot RC Library
 #include <PID.h> // ArduPilot Mega RC Library
 #include <Arduino_Mega_ISR_Registry.h>
@@ -21,7 +22,7 @@ Arduino_Mega_ISR_Registry isr_registry;
     APM_RC_APM1 APM_RC;
 #endif
 
-PID pid(AP_Var::k_key_none, NULL);
+PID pid;
 
 void setup()
 {
@@ -51,7 +52,7 @@ void loop()
 	delay(20);
 	//rc.read_pwm();
 	long error 		= APM_RC.InputCh(0) - radio_trim;
-	long control 	= pid.get_pid(error, 20, 1);
+	long control 	= pid.get_pid(error, 1);
 
 	Serial.print("control: ");
 	Serial.println(control,DEC);