Merge pull request #1906 from mlyle/mpl-deetee

flight: pid refactor: move dt to configuration time

flight/Libraries/math/pid.c

@@ -43,11 +43,12 @@ static float deriv_gamma = 1.0;
  * Update the PID computation
  * @param[in] pid The PID struture which stores temporary information
  * @param[in] err The error term
- * @param[in] dT  The time step
  * @returns Output the computed controller value
  */
-float pid_apply(struct pid *pid, const float err, float dT)
-{	
+float pid_apply(struct pid *pid, const float err)
+{
+	float dT = pid->dT;
+
 	if (pid->i == 0) {
 		// If Ki is zero, do not change the integrator. We do not reset to zero
 		// because sometimes the accumulator term is set externally
@@ -82,8 +83,10 @@ float pid_apply(struct pid *pid, const float err, float dT)
  *  chapter.
  */
 float pid_apply_antiwindup(struct pid *pid, const float err,
-	float min_bound, float max_bound, float dT)
-{	
+	float min_bound, float max_bound)
+{
+	float dT = pid->dT;
+
 	if (pid->i == 0) {
 		// If Ki is zero, do not change the integrator. We do not reset to zero
 		// because sometimes the accumulator term is set externally
@@ -123,15 +126,16 @@ float pid_apply_antiwindup(struct pid *pid, const float err,
  * @param[in] pid The PID struture which stores temporary information
  * @param[in] setpoint The setpoint to use
  * @param[in] measured The measured value of output
- * @param[in] dT  The time step
  * @returns Output the computed controller value
  *
  * This version of apply uses setpoint weighting for the derivative component so the gain
  * on the gyro derivative can be different than the gain on the setpoint derivative
  */
 float pid_apply_setpoint(struct pid *pid, struct pid_deadband *deadband, const float setpoint,
-	const float measured, float dT)
+	const float measured)
 {
+	float dT = pid->dT;
+
 	float err = setpoint - measured;
 	float err_d = (deriv_gamma * setpoint - measured);
 
@@ -196,7 +200,7 @@ void pid_configure_derivative(float cutoff, float g)
  * @param[in] i The integral term
  * @param[in] d The derivative term
  */
-void pid_configure(struct pid *pid, float p, float i, float d, float iLim)
+void pid_configure(struct pid *pid, float p, float i, float d, float iLim, float dT)
 {
 	if (!pid)
 		return;
@@ -205,6 +209,7 @@ void pid_configure(struct pid *pid, float p, float i, float d, float iLim)
 	pid->i = i;
 	pid->d = d;
 	pid->iLim = iLim;
+	pid->dT = dT;
 }
 
 /**

flight/Libraries/math/pid.h

@@ -44,17 +44,20 @@ struct pid {
 	float i;
 	float d;
 	float iLim;
+
+	float dT;
+
 	float iAccumulator;
 	float lastErr;
 	float lastDer;
 };
 
 //! Methods to use the pid structures
-float pid_apply(struct pid *pid, const float err, float dT);
-float pid_apply_antiwindup(struct pid *pid, const float err, float min_bound, float max_bound, float dT);
-float pid_apply_setpoint(struct pid *pid, struct pid_deadband *deadband, const float setpoint, const float measured, float dT);
+float pid_apply(struct pid *pid, const float err);
+float pid_apply_antiwindup(struct pid *pid, const float err, float min_bound, float max_bound);
+float pid_apply_setpoint(struct pid *pid, struct pid_deadband *deadband, const float setpoint, const float measured);
 void pid_zero(struct pid *pid);
-void pid_configure(struct pid *pid, float p, float i, float d, float iLim);
+void pid_configure(struct pid *pid, float p, float i, float d, float iLim, float dT);
 void pid_configure_derivative(float cutoff, float gamma);
 void pid_configure_deadband(struct pid_deadband *deadband, float width, float slope);
 

flight/Modules/AltitudeHold/altitudehold.c

@@ -160,15 +160,16 @@ static void altitudeHoldTask(void *parameters)
 	FlightStatusConnectQueue(queue);
 
 	AltitudeHoldSettingsGet(&altitudeHoldSettings);
+	// Main task loop
+	const uint32_t dt_ms = 20;
+
 	pid_configure(&velocity_pid, altitudeHoldSettings.VelocityKp,
-		          altitudeHoldSettings.VelocityKi, 0.0f, 1.0f);
+		          altitudeHoldSettings.VelocityKi, 0.0f, 1.0f,
+			  dt_ms / 1000.0f);
 
 	AlarmsSet(SYSTEMALARMS_ALARM_ALTITUDEHOLD, SYSTEMALARMS_ALARM_OK);
 
-	// Main task loop
-	const uint32_t dt_ms = 20;
 	uint32_t timeout = dt_ms;
-	uint32_t timeval = PIOS_DELAY_GetRaw();
 
 	while (1) {
 		if (PIOS_Queue_Receive(queue, &ev, timeout) != true) {
@@ -182,8 +183,6 @@ static void altitudeHoldTask(void *parameters)
 				// Copy the current throttle as a starting point for integral
 				StabilizationDesiredThrustGet(&velocity_pid.iAccumulator);
 				engaged = true;
-				timeval = PIOS_DELAY_GetRaw();
-
 			} else if (flight_mode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD)
 				engaged = false;
 
@@ -195,8 +194,10 @@ static void altitudeHoldTask(void *parameters)
 		} else if (ev.obj == AltitudeHoldSettingsHandle()) {
 			AltitudeHoldSettingsGet(&altitudeHoldSettings);
 
-			pid_configure(&velocity_pid, altitudeHoldSettings.VelocityKp,
-				          altitudeHoldSettings.VelocityKi, 0.0f, 1.0f);
+			pid_configure(&velocity_pid,
+					altitudeHoldSettings.VelocityKp,
+					altitudeHoldSettings.VelocityKi,
+					0.0f, 1.0f, dt_ms / 1000.0f);
 			continue;
 		}
 
@@ -219,14 +220,11 @@ static void altitudeHoldTask(void *parameters)
 			// to stop winding up
 			const float min_throttle = landing ? -0.1f : 0.0f;
 
-			// Velocity desired is from the outer controller plus the set point
-			float dT = PIOS_DELAY_DiffuS(timeval) * 1.0e-6f;
-			timeval = PIOS_DELAY_GetRaw();
 			float velocity_desired = altitude_error * altitudeHoldSettings.PositionKp + altitudeHoldDesired.ClimbRate;
 			float throttle_desired = pid_apply_antiwindup(&velocity_pid, 
 			                    velocity_desired - velocity_z,
-			                    min_throttle, 1.0f, // positive limits since this is throttle
-			                    dT);
+			                    min_throttle, 1.0f // positive limits since this is throttle
+			                    );
 
 			AltitudeHoldStateData altitudeHoldState;
 			altitudeHoldState.VelocityDesired = velocity_desired;