changed PI Library to take Float for DeltaTime

Changed timing to be more accurate in main loop. Still need to switch to micros(). Left that for Tridge. This gets rid of six floating point calcs *250. Nice!

ArduCopter/ArduCopter.pde

@@ -490,9 +490,9 @@ static byte				gps_watchdog;
 static unsigned long 	fast_loopTimer;				// Time in miliseconds of main control loop
 static uint8_t 			delta_ms_fast_loop; 		// Delta Time in miliseconds
 
-static unsigned long 	medium_loopTimer;			// Time in miliseconds of navigation control loop
 static byte 			medium_loopCounter;			// Counters for branching from main control loop to slower loops
-static uint8_t			delta_ms_medium_loop;
+static uint16_t			throttle_timer;
+static float			delta_throttle;
 
 static unsigned long	fiftyhz_loopTimer;
 static uint8_t			delta_ms_fiftyhz;
@@ -502,7 +502,7 @@ static int 				superslow_loopCounter;
 static byte				simple_timer;				// for limiting the execution of flight mode thingys
 
 
-static unsigned long 	dTnav;						// Delta Time in milliseconds for navigation computations
+static float 			dTnav;						// Delta Time in milliseconds for navigation computations
 static unsigned long 	nav_loopTimer;				// used to track the elapsed ime for GPS nav
 //static float 			load;						// % MCU cycles used
 
@@ -526,9 +526,6 @@ void loop()
 	// ----------------------------
 	if (millis() - fast_loopTimer >= 4) {
 		//PORTK |= B00010000;
-		delta_ms_fast_loop 	= millis() - fast_loopTimer;
-		fast_loopTimer		= millis();
-		G_Dt 				= (float)delta_ms_fast_loop / 1000.f;		// used by DCM integrator
 
 		// Execute the fast loop
 		// ---------------------
@@ -573,18 +570,6 @@ void loop()
 // Main loop
 static void fast_loop()
 {
-	// IMU DCM Algorithm
-	read_AHRS();
-
-	// This is the fast loop - we want it to execute at >= 100Hz
-	// ---------------------------------------------------------
-	if (delta_ms_fast_loop > G_Dt_max)
-		G_Dt_max = delta_ms_fast_loop;
-
-	// Read radio
-	// ----------
-	read_radio();
-
     // try to send any deferred messages if the serial port now has
     // some space available
     gcs.send_message(MSG_RETRY_DEFERRED);
@@ -592,6 +577,22 @@ static void fast_loop()
     hil.send_message(MSG_RETRY_DEFERRED);
 #endif
 
+	// Read radio
+	// ----------
+	read_radio();
+
+	// IMU DCM Algorithm
+	read_AHRS();
+
+	fast_loopTimer		= millis();
+	delta_ms_fast_loop 	= millis() - fast_loopTimer;
+	G_Dt 				= (float)delta_ms_fast_loop / 1000.f;		// used by PI Loops
+
+	// Look for slow loop times
+	// ------------------------
+	if (delta_ms_fast_loop > G_Dt_max)
+		G_Dt_max = delta_ms_fast_loop;
+
 	// custom code/exceptions for flight modes
 	// ---------------------------------------
 	update_yaw_mode();
@@ -663,7 +664,7 @@ static void medium_loop()
 				g_gps->new_data 	= false;
 
 				// we are not tracking I term on navigation, so this isn't needed
-				dTnav 				= millis() - nav_loopTimer;
+				dTnav 				= (float)(millis() - nav_loopTimer)/ 1000.0;
 				nav_loopTimer 		= millis();
 
 				// calculate the copter's desired bearing and WP distance
@@ -745,9 +746,6 @@ static void medium_loop()
 			loop_step = 5;
 			medium_loopCounter = 0;
 
-			delta_ms_medium_loop	= millis() - medium_loopTimer;
-			medium_loopTimer    	= millis();
-
 			if (g.battery_monitoring != 0){
 				read_battery();
 			}
@@ -1190,7 +1188,6 @@ static void update_navigation()
 			// calculates desired Yaw
 			update_auto_yaw();
 			{
-				//circle_angle += dTnav; //1000 * (dTnav/1000);
 				circle_angle = wrap_360(target_bearing + 3000 + 18000);
 
 				target_WP.lng = next_WP.lng + (g.loiter_radius * cos(radians(90 - circle_angle/100)));

ArduCopter/Attitude.pde

@@ -13,13 +13,13 @@ get_stabilize_roll(long target_angle)
 	error 		= constrain(error, -2500, 2500);
 
 	// desired Rate:
-	rate 		= g.pi_stabilize_roll.get_pi(error, delta_ms_fast_loop);
+	rate 		= g.pi_stabilize_roll.get_pi(error, G_Dt);
 	//Serial.printf("%d\t%d\t%d ", (int)target_angle, (int)error, (int)rate);
 
 #if FRAME_CONFIG != HELI_FRAME  // cannot use rate control for helicopters
 	// Rate P:
 	error 		= rate - (long)(degrees(omega.x) * 100.0);
-	rate 		= g.pi_rate_roll.get_pi(error, delta_ms_fast_loop);
+	rate 		= g.pi_rate_roll.get_pi(error, G_Dt);
 	//Serial.printf("%d\t%d\n", (int)error, (int)rate);
 #endif
 
@@ -40,13 +40,13 @@ get_stabilize_pitch(long target_angle)
 	error 		= constrain(error, -2500, 2500);
 
 	// desired Rate:
-	rate 		= g.pi_stabilize_pitch.get_pi(error, delta_ms_fast_loop);
+	rate 		= g.pi_stabilize_pitch.get_pi(error, G_Dt);
 	//Serial.printf("%d\t%d\t%d ", (int)target_angle, (int)error, (int)rate);
 
 #if FRAME_CONFIG != HELI_FRAME  // cannot use rate control for helicopters
 	// Rate P:
 	error 		= rate - (long)(degrees(omega.y) * 100.0);
-	rate 		= g.pi_rate_pitch.get_pi(error, delta_ms_fast_loop);
+	rate 		= g.pi_rate_pitch.get_pi(error, G_Dt);
 	//Serial.printf("%d\t%d\n", (int)error, (int)rate);
 #endif
 
@@ -66,19 +66,19 @@ get_stabilize_yaw(long target_angle)
 
 	// limit the error we're feeding to the PID
 	yaw_error 		= constrain(yaw_error, -YAW_ERROR_MAX, YAW_ERROR_MAX);
-	rate 			= g.pi_stabilize_yaw.get_pi(yaw_error, delta_ms_fast_loop);
+	rate 			= g.pi_stabilize_yaw.get_pi(yaw_error, G_Dt);
 	//Serial.printf("%u\t%d\t%d\t", (int)target_angle, (int)error, (int)rate);
 
 #if FRAME_CONFIG == HELI_FRAME  // cannot use rate control for helicopters
 	if( ! g.heli_ext_gyro_enabled ) {
 		// Rate P:
 		error 		= rate - (long)(degrees(omega.z) * 100.0);
-		rate 		= g.pi_rate_yaw.get_pi(error, delta_ms_fast_loop);
+		rate 		= g.pi_rate_yaw.get_pi(error, G_Dt);
 	}
 #else
 	// Rate P:
 	error 			= rate - (long)(degrees(omega.z) * 100.0);
-	rate 			= g.pi_rate_yaw.get_pi(error, delta_ms_fast_loop);
+	rate 			= g.pi_rate_yaw.get_pi(error, G_Dt);
 	//Serial.printf("%d\t%d\n", (int)error, (int)rate);
 #endif
 
@@ -100,7 +100,10 @@ get_nav_throttle(long z_error, int target_speed)
 	rate_error 		= target_speed - altitude_rate;
 	rate_error 		= constrain(rate_error, -110, 110);
 
-	return g.pi_throttle.get_pi(rate_error, delta_ms_medium_loop);
+	delta_throttle		= (float)(millis() - throttle_timer)/1000.0;
+	throttle_timer    	= millis();
+
+	return g.pi_throttle.get_pi(rate_error, delta_throttle);
 }
 
 static int
@@ -110,7 +113,7 @@ get_rate_roll(long target_rate)
 	target_rate 		= constrain(target_rate, -2500, 2500);
 
 	error		= (target_rate * 4.5) - (long)(degrees(omega.x) * 100.0);
-	target_rate = g.pi_rate_roll.get_pi(error, delta_ms_fast_loop);
+	target_rate = g.pi_rate_roll.get_pi(error, G_Dt);
 
 	// output control:
 	return (int)constrain(target_rate, -2500, 2500);
@@ -123,7 +126,7 @@ get_rate_pitch(long target_rate)
 	target_rate 		= constrain(target_rate, -2500, 2500);
 
 	error		= (target_rate * 4.5) - (long)(degrees(omega.y) * 100.0);
-	target_rate = g.pi_rate_pitch.get_pi(error, delta_ms_fast_loop);
+	target_rate = g.pi_rate_pitch.get_pi(error, G_Dt);
 
 	// output control:
 	return (int)constrain(target_rate, -2500, 2500);
@@ -135,7 +138,7 @@ get_rate_yaw(long target_rate)
 	long error;
 
 	error		= (target_rate * 4.5) - (long)(degrees(omega.z) * 100.0);
-	target_rate = g.pi_rate_yaw.get_pi(error, delta_ms_fast_loop);
+	target_rate = g.pi_rate_yaw.get_pi(error, G_Dt);
 
 	// output control:
 	return (int)constrain(target_rate, -2500, 2500);

ArduCopter/sensors.pde

@@ -122,7 +122,7 @@ static void read_battery(void)
 
 	if(g.battery_monitoring == 4) {
 		current_amps	 = CURRENT_AMPS(analogRead(CURRENT_PIN_1)) * .1 + current_amps * .9; //reads power sensor current pin
-		current_total	 += current_amps * (float)delta_ms_medium_loop * 0.000278;
+		current_total	 += current_amps * 0.0278;	// called at 100ms on average
 	}
 
 	#if BATTERY_EVENT == 1

ArduCopter/test.pde

@@ -649,7 +649,7 @@ static int8_t
 test_current(uint8_t argc, const Menu::arg *argv)
 {
 	print_hit_enter();
-	delta_ms_medium_loop = 100;
+	//delta_ms_medium_loop = 100;
 
 	while(1){
 		delay(100);

libraries/APM_PI/APM_PI.cpp

@@ -8,16 +8,16 @@
 #include "APM_PI.h"
 
 long
-APM_PI::get_pi(int32_t error, uint16_t dt)
+APM_PI::get_pi(int32_t error, float dt)
 {
 	float output		= 0;
- 	float delta_time	= (float)dt / 1000.0;
+ 	//float delta_time	= (float)dt / 1000.0;
 
 	// Compute proportional component
 	output += error * _kp;
 
 	// Compute integral component if time has elapsed
-	_integrator += (error * _ki) * delta_time;
+	_integrator += (error * _ki) * dt;
 
 	if (_integrator < -_imax) {
 		_integrator = -_imax;
@@ -26,7 +26,6 @@ APM_PI::get_pi(int32_t error, uint16_t dt)
 	}
 
 	output += _integrator;
-
 	return output;
 }
 

libraries/APM_PI/APM_PI.h

@@ -77,7 +77,7 @@ public:
 	///
 	/// @returns		The updated control output.
 	///
-	long 	get_pi(int32_t error, uint16_t dt);
+	long 	get_pi(int32_t error, float dt);
 
 	/// Reset the PI integrator
 	///