Added rate PID control.

1 files changed, 51 insertions, 7 deletions

diff --git a/main.cpp b/main.cpp
index 6bd8f6e..c0deaf9 100644
--- a/main.cpp
+++ b/main.cpp
@@ -6,6 +6,41 @@
 
 #include "itg3200.h"
 
+template<class T>
+inline void saturate(T& var, T absmax) {
+	if(var > absmax) {
+		var = absmax;
+	} else if(var < -absmax) {
+		var = -absmax;
+	}
+}
+
+class PID {
+	private:
+		uint16_t Kp, Ki, Kd;
+		
+		int16_t last;
+		int32_t accum;
+	
+	public:
+		PID(uint16_t p, uint16_t i, uint16_t d) : Kp(p), Ki(i), Kd(d), last(0), accum(0) {}
+		
+		int16_t update(int16_t error) {
+			// P
+			int32_t corr_p = Kp * error;
+			
+			// I
+			accum += Ki * error;
+			int32_t corr_i = accum;
+			
+			// D
+			int32_t corr_d = Kd * (error - last);
+			last = error;
+			
+			return (corr_p + corr_i + corr_d) >> 16;
+		}
+};
+
 int main() {
 	RCC.enable(RCC.AFIO);
 	RCC.enable(RCC.IOPA);
@@ -35,6 +70,10 @@ int main() {
 	
 	TIM2.CR1 = 0x05;
 	
+	PID pid_pitch(2000, 0, 0);
+	PID pid_roll(2000, 0, 0);
+	PID pid_yaw(2000, 0, 0);
+	
 	while(1) {
 		// Wait for a new update.
 		while(!(TIM2.SR & 0x01));
@@ -47,13 +86,18 @@ int main() {
 		
 		// Generate motor mix.
 		int16_t throttle = ppmsum.channels[2] - 1000;
-		int16_t pitch = 0;
-		int16_t yaw = gyro.z >> 7;
-		int16_t roll = 0;
+		int16_t pitch = pid_pitch.update((ppmsum.channels[1] - 1500) - gyro.x);
+		int16_t roll = pid_roll.update((ppmsum.channels[0] - 1500) - gyro.y);
+		int16_t yaw = pid_yaw.update((ppmsum.channels[3] - 1500) - gyro.z);
+		
+		int16_t max = throttle > 250 ? 250 : throttle;
+		saturate(pitch, max);
+		saturate(roll, max);
+		saturate(yaw, max);
 		
-		TIM2.CCR1 = 1000 + throttle + pitch + roll - yaw;
-		TIM2.CCR2 = 1000 + throttle + pitch - roll + yaw;
-		TIM2.CCR3 = 1000 + throttle - pitch + roll + yaw;
-		TIM2.CCR4 = 1000 + throttle - pitch - roll - yaw;
+		TIM2.CCR1 = 1000 + throttle + pitch + roll + yaw;
+		TIM2.CCR2 = 1000 + throttle + pitch - roll - yaw;
+		TIM2.CCR3 = 1000 + throttle - pitch + roll - yaw;
+		TIM2.CCR4 = 1000 + throttle - pitch - roll + yaw;
 	}
 }