Move the PID methods into a standalone library

flight/CopterControl/Makefile

@@ -115,6 +115,8 @@ OPTESTS  = ./Tests
 OPMODULEDIR = ../Modules
 FLIGHTLIB = ../Libraries
 FLIGHTLIBINC = $(FLIGHTLIB)/inc
+MATHLIB = ../Libraries/math
+MATHLIBINC = ../Libraries/math
 PIOS = ../PiOS
 PIOSINC = $(PIOS)/inc
 PIOSSTM32F10X = $(PIOS)/STM32F10x
@@ -268,8 +270,9 @@ SRC += $(PIOSCOMMON)/printf-stdarg.c
 ## Libraries for flight calculations
 SRC += $(FLIGHTLIB)/fifo_buffer.c
 SRC += $(FLIGHTLIB)/CoordinateConversions.c
-SRC += $(FLIGHTLIB)/math/sin_lookup.c
 SRC += $(FLIGHTLIB)/taskmonitor.c
+SRC += $(MATHLIB)/sin_lookup.c
+SRC += $(MATHLIB)/pid.c
 
 ## CMSIS for STM32
 SRC += $(CMSISDIR)/core_cm3.c
@@ -370,7 +373,7 @@ EXTRAINCDIRS  += $(OPUAVSYNTHDIR)
 EXTRAINCDIRS  += $(PIOS)
 EXTRAINCDIRS  += $(PIOSINC)
 EXTRAINCDIRS  += $(FLIGHTLIBINC)
-EXTRAINCDIRS  += $(FLIGHTLIB)/math
+EXTRAINCDIRS  += $(MATHLIBINC)
 EXTRAINCDIRS  += $(PIOSSTM32F10X)
 EXTRAINCDIRS  += $(PIOSCOMMON)
 EXTRAINCDIRS  += $(PIOSBOARDS)

flight/Libraries/math/pid.c

@@ -0,0 +1,91 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilot Math Utilities
+ * @{
+ * @addtogroup Sine and cosine methods that use a cached lookup table
+ * @{
+ *
+ * @file       pid.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @brief      Methods to work with PID structure
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include "openpilot.h"
+#include "pid.h"
+
+//! Private method
+static float bound(float val, float range);
+
+float pid_apply(struct pid *pid, const float err, float dT)
+{
+	float diff = (err - pid->lastErr);
+	pid->lastErr = err;
+	
+	// Scale up accumulator by 1000 while computing to avoid losing precision
+	pid->iAccumulator += err * (pid->i * dT * 1000.0f);
+	pid->iAccumulator = bound(pid->iAccumulator, pid->iLim * 1000.0f);
+	return ((err * pid->p) + pid->iAccumulator / 1000.0f + (diff * pid->d / dT));
+}
+
+/**
+ * Reset a bit
+ * @param[in] pid The pid to reset
+ */
+void pid_zero(struct pid *pid)
+{
+	if (!pid)
+		return;
+
+	pid->iAccumulator = 0;
+	pid->lastErr = 0;
+}
+
+/**
+ * Configure the settings for a pid structure
+ * @param[out] pid The PID structure to configure
+ * @param[in] p The proportional term
+ * @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)
+{
+	if (!pid)
+		return;
+
+	pid->p = p;
+	pid->i = i;
+	pid->d = d;
+	pid->iLim = iLim;
+}
+
+/**
+ * Bound input value between limits
+ */
+static float bound(float val, float range)
+{
+	if(val < -range) {
+		val = -range;
+	} else if(val > range) {
+		val = range;
+	}
+	return val;
+}
+

flight/Libraries/math/pid.h

@@ -0,0 +1,49 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilot Math Utilities
+ * @{
+ * @addtogroup Sine and cosine methods that use a cached lookup table
+ * @{
+ *
+ * @file       pid.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @brief      Methods to work with PID structure
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#ifndef PID_H
+#define PID_H
+
+//! 
+struct pid {
+	float p;
+	float i;
+	float d;
+	float iLim;
+	float iAccumulator;
+	float lastErr;
+};
+
+//! Methods to use the pid structures
+float pid_apply(struct pid *pid, const float err, float dT);
+void pid_zero(struct pid *pid);
+void pid_configure(struct pid *pid, float p, float i, float d, float iLim);
+
+#endif /* PID_H */

flight/Modules/Stabilization/stabilization.c

@@ -43,7 +43,10 @@
 #include "gyros.h"
 #include "flightstatus.h"
 #include "manualcontrol.h" // Just to get a macro
+
+// Math libraries
 #include "CoordinateConversions.h"
+#include "pid.h"
 
 // Includes for various stabilization algorithms
 #include "relay_tuning.h"
@@ -61,17 +64,7 @@
 #define TASK_PRIORITY (tskIDLE_PRIORITY+4)
 #define FAILSAFE_TIMEOUT_MS 30
 
-enum {PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_VBAR_ROLL, PID_VBAR_PITCH, PID_VBAR_YAW, PID_MAX};
+enum {PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_MAX};
-
-// Private types
-typedef struct {
-	float p;
-	float i;
-	float d;
-	float iLim;
-	float iAccumulator;
-	float lastErr;
-} pid_type;
 
 // Private variables
 static xTaskHandle taskHandle;
@@ -79,18 +72,16 @@ static StabilizationSettingsData settings;
 static xQueueHandle queue;
 float gyro_alpha = 0;
 float axis_lock_accum[3] = {0,0,0};
-float vbar_sensitivity[3] = {1, 1, 1};
 uint8_t max_axis_lock = 0;
 uint8_t max_axislock_rate = 0;
 float weak_leveling_kp = 0;
 uint8_t weak_leveling_max = 0;
 bool lowThrottleZeroIntegral;
 float vbar_decay = 0.991f;
-pid_type pids[PID_MAX];
+struct pid pids[PID_MAX];
 
 // Private functions
 static void stabilizationTask(void* parameters);
-static float ApplyPid(pid_type * pid, const float err, float dT);
 static float bound(float val, float range);
 static void ZeroPids(void);
 static void SettingsUpdatedCb(UAVObjEvent * ev);
@@ -251,7 +242,7 @@ static void stabilizationTask(void* parameters)
 					rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate[i]);
 
 					// Compute the inner loop
-					actuatorDesiredAxis[i] = ApplyPid(&pids[PID_RATE_ROLL + i],  rateDesiredAxis[i] - gyro_filtered[i], dT);
+					actuatorDesiredAxis[i] = pid_apply(&pids[PID_RATE_ROLL + i],  rateDesiredAxis[i] - gyro_filtered[i], dT);
 					actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f);
 
 					break;
@@ -263,11 +254,11 @@ static void stabilizationTask(void* parameters)
 					}
 
 					// Compute the outer loop
-					rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], local_error[i], dT);
+					rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT);
 					rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]);
 
 					// Compute the inner loop
-					actuatorDesiredAxis[i] = ApplyPid(&pids[PID_RATE_ROLL + i],  rateDesiredAxis[i] - gyro_filtered[i], dT);
+					actuatorDesiredAxis[i] = pid_apply(&pids[PID_RATE_ROLL + i],  rateDesiredAxis[i] - gyro_filtered[i], dT);
 					actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f);
 
 					break;
@@ -291,7 +282,7 @@ static void stabilizationTask(void* parameters)
 
 					// Compute desired rate as input biased towards leveling
 					rateDesiredAxis[i] = attitudeDesiredAxis[i] + weak_leveling;
-					actuatorDesiredAxis[i] = ApplyPid(&pids[PID_RATE_ROLL + i],  rateDesiredAxis[i] - gyro_filtered[i], dT);
+					actuatorDesiredAxis[i] = pid_apply(&pids[PID_RATE_ROLL + i],  rateDesiredAxis[i] - gyro_filtered[i], dT);
 					actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f);
 
 					break;
@@ -308,12 +299,12 @@ static void stabilizationTask(void* parameters)
 						// For weaker commands or no command simply attitude lock (almost) on no gyro change
 						axis_lock_accum[i] += (attitudeDesiredAxis[i] - gyro_filtered[i]) * dT;
 						axis_lock_accum[i] = bound(axis_lock_accum[i], max_axis_lock);
-						rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], axis_lock_accum[i], dT);
+						rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], axis_lock_accum[i], dT);
 					}
 
 					rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]);
 
-					actuatorDesiredAxis[i] = ApplyPid(&pids[PID_RATE_ROLL + i],  rateDesiredAxis[i] - gyro_filtered[i], dT);
+					actuatorDesiredAxis[i] = pid_apply(&pids[PID_RATE_ROLL + i],  rateDesiredAxis[i] - gyro_filtered[i], dT);
 					actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f);
 
 					break;
@@ -333,7 +324,7 @@ static void stabilizationTask(void* parameters)
 						pids[PID_ROLL + i].iAccumulator = 0;
 
 					// Compute the outer loop like attitude mode
-					rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], local_error[i], dT);
+					rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT);
 					rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]);
 
 					// Run the relay controller which also estimates the oscillation parameters
@@ -387,33 +378,15 @@ static void stabilizationTask(void* parameters)
 	}
 }
 
-/**
- * Update one of the PID structures with the input error and timestep
- * @param pid Pointer to the PID structure
- * @param[in] err The error on for this controller
- * @param[in] dT The time step since the last update
- */
-float ApplyPid(pid_type * pid, const float err, float dT)
-{
-	float diff = (err - pid->lastErr);
-	pid->lastErr = err;
-	
-	// Scale up accumulator by 1000 while computing to avoid losing precision
-	pid->iAccumulator += err * (pid->i * dT * 1000.0f);
-	pid->iAccumulator = bound(pid->iAccumulator, pid->iLim * 1000.0f);
-	return ((err * pid->p) + pid->iAccumulator / 1000.0f + (diff * pid->d / dT));
-}
-
 
 /**
  * Clear the accumulators and derivatives for all the axes
  */
 static void ZeroPids(void)
 {
-	for(int8_t ct = 0; ct < PID_MAX; ct++) {
+	for(uint32_t i = 0; i < PID_MAX; i++)
-		pids[ct].iAccumulator = 0.0f;
+		pid_zero(&pids[i]);
-		pids[ct].lastErr = 0.0f;
+
-	}
 	for(uint8_t i = 0; i < 3; i++)
 		axis_lock_accum[i] = 0.0f;
 }
@@ -437,54 +410,37 @@ static void SettingsUpdatedCb(UAVObjEvent * ev)
 	StabilizationSettingsGet(&settings);
 	
 	// Set the roll rate PID constants
-	pids[PID_RATE_ROLL].p = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP];
+	pid_configure(&pids[PID_RATE_ROLL], settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], 
-	pids[PID_RATE_ROLL].i = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI];
+		settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI],
-	pids[PID_RATE_ROLL].d = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD];
+		pids[PID_RATE_ROLL].d = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD],
-	pids[PID_RATE_ROLL].iLim = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT];
+		pids[PID_RATE_ROLL].iLim = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT]);
 	
 	// Set the pitch rate PID constants
-	pids[PID_RATE_PITCH].p = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP];
+	pid_configure(&pids[PID_RATE_PITCH], settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], 
-	pids[PID_RATE_PITCH].i = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI];
+		pids[PID_RATE_PITCH].i = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI],
-	pids[PID_RATE_PITCH].d = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD];
+		pids[PID_RATE_PITCH].d = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD],
-	pids[PID_RATE_PITCH].iLim = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT];
+		pids[PID_RATE_PITCH].iLim = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT]);
 	
 	// Set the yaw rate PID constants
-	pids[PID_RATE_YAW].p = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP];
+	pid_configure(&pids[PID_RATE_YAW], settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP],
-	pids[PID_RATE_YAW].i = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI];
+		pids[PID_RATE_YAW].i = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI],
-	pids[PID_RATE_YAW].d = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD];
+		pids[PID_RATE_YAW].d = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD],
-	pids[PID_RATE_YAW].iLim = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT];
+		pids[PID_RATE_YAW].iLim = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT]);
 	
 	// Set the roll attitude PI constants
-	pids[PID_ROLL].p = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP];
+	pid_configure(&pids[PID_ROLL], settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP],
-	pids[PID_ROLL].i = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI];
+		settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], 0,
-	pids[PID_ROLL].iLim = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT];
+		pids[PID_ROLL].iLim = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT]);
 	
 	// Set the pitch attitude PI constants
-	pids[PID_PITCH].p = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP];
+	pid_configure(&pids[PID_PITCH], settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP],
-	pids[PID_PITCH].i = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI];
+		pids[PID_PITCH].i = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], 0,
-	pids[PID_PITCH].iLim = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT];
+		settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT]);
 	
 	// Set the yaw attitude PI constants
-	pids[PID_YAW].p = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KP];
+	pid_configure(&pids[PID_YAW], settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KP],
-	pids[PID_YAW].i = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KI];
+		settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KI], 0,
-	pids[PID_YAW].iLim = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT];
+		settings.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT]);
-	
-	// Set the roll attitude PI constants
-	pids[PID_VBAR_ROLL].p = settings.VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KP];
-	pids[PID_VBAR_ROLL].i = settings.VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KI];
-	
-	// Set the pitch attitude PI constants
-	pids[PID_VBAR_PITCH].p = settings.VbarPitchPI[STABILIZATIONSETTINGS_VBARPITCHPI_KP];
-	pids[PID_VBAR_PITCH].i = settings.VbarPitchPI[STABILIZATIONSETTINGS_VBARPITCHPI_KI];
-	
-	// Set the yaw attitude PI constants
-	pids[PID_VBAR_YAW].p = settings.VbarYawPI[STABILIZATIONSETTINGS_VBARYAWPI_KP];
-	pids[PID_VBAR_YAW].i = settings.VbarYawPI[STABILIZATIONSETTINGS_VBARYAWPI_KI];
-	
-	// Need to store the vbar sensitivity
-	vbar_sensitivity[0] = settings.VbarSensitivity[0];
-	vbar_sensitivity[1] = settings.VbarSensitivity[1];
-	vbar_sensitivity[2] = settings.VbarSensitivity[2];
 	
 	// Maximum deviation to accumulate for axis lock
 	max_axis_lock = settings.MaxAxisLock;