The unified diff between revisions [64de686d..] and [24d5b9f4..] is displayed below. It can also be downloaded as a raw diff.

#
# old_revision [64de686d701acb9539dc52fe0bff299405612ab0]
# new_revision [24d5b9f4dff9135787b198fe1127d9c1e3326b9c]
#
# add_file "thrust.c"
#  content [cd4c74340b2aaabdc4543f6334d5c44f287e0965]
# 
# add_file "thrust.h"
#  content [da150443af77f614863c04338125e4c836aacd42]
# 
# patch "Makefile"
#  from [3c0587750b990cb4dc04f8caccec5088db10298c]
#    to [7a1a49fb3e2f7d3f482e493962f2f02dae51aa93]
# 
# patch "main.c"
#  from [7098e8a2414c2d385b9a43d19c2e3b2461ae00c0]
#    to [8284ca89102568b2ae6f26e057ab296621f87e6a]
# 
# patch "motor.c"
#  from [b9920bb4788532a73cb93301787e4d0540cc3d56]
#    to [0a5cff753a89545c1d3f23027cfb4f651db669cd]
#
============================================================
--- Makefile	3c0587750b990cb4dc04f8caccec5088db10298c
+++ Makefile	7a1a49fb3e2f7d3f482e493962f2f02dae51aa93
@@ -5,6 +5,7 @@ CSRCS+=fisqrt.c stick.c trig.c motor.c l
 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	7098e8a2414c2d385b9a43d19c2e3b2461ae00c0
+++ main.c	8284ca89102568b2ae6f26e057ab296621f87e6a
@@ -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	b9920bb4788532a73cb93301787e4d0540cc3d56
+++ motor.c	0a5cff753a89545c1d3f23027cfb4f651db669cd
@@ -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 throttle = 0.0f;
 
 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
 	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
 		}
 	}
 
-	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) {
============================================================
--- /dev/null	
+++ thrust.c	cd4c74340b2aaabdc4543f6334d5c44f287e0965
@@ -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));
+}
+
============================================================
--- /dev/null	
+++ thrust.h	da150443af77f614863c04338125e4c836aacd42
@@ -0,0 +1,4 @@
+/* thrust.h */
+
+float linearise_thrust(float x);
+void set_thrust(int motor, float x);