From 274805c2a02550cbc993ad12e3307803226793cd Mon Sep 17 00:00:00 2001
From: Gavan Fantom <gavan@coolfactor.org>
Date: Sat, 8 Dec 2012 12:31:17 +0000
Subject: [PATCH] Limit integral windup, some new PID values and linearise
 thrust.

---
 Makefile |  1 +
 main.c   | 48 ++++++++++++++++++++++++++++++++++++------------
 motor.c  | 30 +++++++++++++++++++-----------
 thrust.c | 15 +++++++++++++++
 thrust.h |  4 ++++
 5 files changed, 75 insertions(+), 23 deletions(-)
 create mode 100644 thrust.c
 create mode 100644 thrust.h

diff --git a/Makefile b/Makefile
index 093d4d9..0233af8 100644
--- a/Makefile
+++ b/Makefile
@@ -5,6 +5,7 @@ NAME=quad
 SSRCS=crt0.s
 CSRCS=main.c i2c.c wmp.c timer.c interrupt.c uart.c event.c matrix.c dcm.c
 CSRCS+=fisqrt.c stick.c trig.c motor.c led.c watchdog.c panic.c status.c
+CSRCS+=thrust.c
 
 #PROJOPTS=-DUSE_UART -DSEND_DCM
 
diff --git a/main.c b/main.c
index 1b75786..5db8358 100644
--- a/main.c
+++ b/main.c
@@ -9,6 +9,7 @@
 #include "led.h"
 #include "status.h"
 #include "watchdog.h"
+#include "thrust.h"
 
 #define PINSEL0 (*((volatile unsigned int *) 0xE002C000))
 #define PINSEL1 (*((volatile unsigned int *) 0xE002C004))
@@ -219,6 +220,8 @@ int main(void) {
 	return 0;
 }
 
+static int power = 0;
+
 void menu_handler(void)
 {
 	int i;
@@ -329,35 +332,56 @@ void menu_handler(void)
 			}
 			break;
 		case '0' & 0xdf:
-			timer_set_pwm_value(0, 0);
-			timer_set_pwm_value(1, 0);
-			timer_set_pwm_value(2, 0);
-			timer_set_pwm_value(3, 0);
+			set_thrust(0, 0.0);
+			set_thrust(1, 0.0);
+			set_thrust(2, 0.0);
+			set_thrust(3, 0.0);
+			power = 0;
+			break;
+#if 0
+		case '1' & 0xdf:
+			power--;
+			if (power < 0)
+				power = 15;
+			power = power % 16;
+			putstr("Power setting: ");
+			putint(power);
+			putstr("\r\n");
+			set_thrust(0, ((float)power)/16.0);
 			break;
+		case '2' & 0xdf:
+			power++;
+			power = power % 16;
+			putstr("Power setting: ");
+			putint(power);
+			putstr("\r\n");
+			set_thrust(0, ((float)power)/16.0);
+			break;
+#endif
 #if 0
 		case '1' & 0xdf:
-			timer_set_pwm_value(0, PWM_MAX/2);
+			set_thrust(0, 0.5);
 			break;
 		case '2' & 0xdf:
-			timer_set_pwm_value(1, PWM_MAX/2);
+			set_thrust(1, 0.5);
 			break;
 		case '3' & 0xdf:
-			timer_set_pwm_value(2, PWM_MAX/2);
+			set_thrust(2, 0.5);
 			break;
 		case '4' & 0xdf:
-			timer_set_pwm_value(3, PWM_MAX/2);
+			set_thrust(3, 0.5);
 			break;
 		case '5' & 0xdf:
-			timer_set_pwm_value(0, PWM_MAX);
+			set_thrust(0, 1.0);
 			break;
 		case '6' & 0xdf:
-			timer_set_pwm_value(1, PWM_MAX);
+			set_thrust(1, 1.0);
 			break;
 		case '7' & 0xdf:
-			timer_set_pwm_value(2, PWM_MAX);
+			set_thrust(2, 1.0);
 			break;
 		case '8' & 0xdf:
-			timer_set_pwm_value(3, PWM_MAX);
+			set_thrust(3, 1.0);
 			break;
 #endif
 		case '9' & 0xdf:
diff --git a/motor.c b/motor.c
index dc76a9d..377e4be 100644
--- a/motor.c
+++ b/motor.c
@@ -1,7 +1,7 @@
 /* motor.c */
 
 #include "stick.h"
-#include "timer.h"
+#include "thrust.h"
 #include "dcm.h"
 #include "uart.h"
 #include "status.h"
@@ -11,8 +11,8 @@ float last[3];
 
 float throttle = 0.0f;
 
-#define Kp 0.2
-#define Ki 0.04
+#define Kp 0.3
+#define Ki 0.02
 #define Kd 0.08
 #define Ka 0.0
 
@@ -57,6 +57,14 @@ void motor_pid_update(float troll, float mroll,
 	integral[1] += pitch * delta_t;
 	integral[2] += yaw * delta_t;
 
+#define INTEGRAL_LIMIT 1.0f
+	for (i = 0; i < 3; i++) {
+		if (integral[i] > INTEGRAL_LIMIT)
+			integral[i] = INTEGRAL_LIMIT;
+		if (integral[i] < -INTEGRAL_LIMIT)
+			integral[i] = -INTEGRAL_LIMIT;
+	}
+
 	/* The measurements are the opposite sign to the error */
 	derivative[0] = (-mroll  - last[0]) / delta_t;
 	derivative[1] = (-mpitch - last[1]) / delta_t;
@@ -133,18 +141,18 @@ void motor_pid_update(float troll, float mroll,
 		}
 	}
 
-	timer_set_pwm_value(0, (int)(motor[0] * PWM_MAX));
-	timer_set_pwm_value(1, (int)(motor[1] * PWM_MAX));
-	timer_set_pwm_value(2, (int)(motor[2] * PWM_MAX));
-	timer_set_pwm_value(3, (int)(motor[3] * PWM_MAX));
+	set_thrust(0, motor[0]);
+	set_thrust(1, motor[1]);
+	set_thrust(2, motor[2]);
+	set_thrust(3, motor[3]);
 }
 
 void motor_kill(void) {
 	throttle = 0.0;
-	timer_set_pwm_value(0, 0);
-	timer_set_pwm_value(1, 0);
-	timer_set_pwm_value(2, 0);
-	timer_set_pwm_value(3, 0);
+	set_thrust(0, 0.0);
+	set_thrust(1, 0.0);
+	set_thrust(2, 0.0);
+	set_thrust(3, 0.0);
 }
 
 void motor_set_throttle(float t) {
diff --git a/thrust.c b/thrust.c
new file mode 100644
index 0000000..ede3e39
--- /dev/null
+++ b/thrust.c
@@ -0,0 +1,15 @@
+/* thrust.c */
+
+#include "fisqrt.h"
+#include "timer.h"
+
+float linearise_thrust(float x)
+{
+	return fisqrt(fisqrt(x*x*x));
+}
+
+void set_thrust(int motor, float x)
+{
+	timer_set_pwm_value(motor, (int)(linearise_thrust(x) * PWM_MAX));
+}
+
diff --git a/thrust.h b/thrust.h
new file mode 100644
index 0000000..7073d15
--- /dev/null
+++ b/thrust.h
@@ -0,0 +1,4 @@
+/* thrust.h */
+
+float linearise_thrust(float x);
+void set_thrust(int motor, float x);