Limit integral windup, some new PID values and linearise thrust.

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
 

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:
-			timer_set_pwm_value(0, PWM_MAX/2);
+			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:
-			timer_set_pwm_value(1, PWM_MAX/2);
+			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:
+			set_thrust(0, 0.5);
+			break;
+		case '2' & 0xdf:
+			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:

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) {

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));
+}
+

thrust.h

@@ -0,0 +1,4 @@
+/* thrust.h */
+
+float linearise_thrust(float x);
+void set_thrust(int motor, float x);