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

This diff has been restricted to the following files: 'motor.c'

#
# old_revision [d0420ebd87c820e33a32b29727989516e15980a8]
# new_revision [24d5b9f4dff9135787b198fe1127d9c1e3326b9c]
#
# patch "motor.c"
#  from [500d9a9173fe983ae7c50cf5fac1d37efa96d8ce]
#    to [0a5cff753a89545c1d3f23027cfb4f651db669cd]
#
============================================================
--- motor.c	500d9a9173fe983ae7c50cf5fac1d37efa96d8ce
+++ motor.c	0a5cff753a89545c1d3f23027cfb4f651db669cd
@@ -1,17 +1,18 @@
 /* motor.c */
 
 #include "stick.h"
-#include "timer.h"
+#include "thrust.h"
 #include "dcm.h"
 #include "uart.h"
+#include "status.h"
 
 float integral[3] = {0.0f, 0.0f, 0.0f};
 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
 
@@ -56,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;
@@ -69,7 +78,7 @@ void motor_pid_update(float troll, float
 	out[1] = pitch * Kp   + integral[1] * Ki   + derivative[1] * Kd;
 	out[2] = yaw   * Kp_y + integral[2] * Ki_y + derivative[2] * Kd_y;
 
-	if (armed) {
+	if (status_armed()) {
 		/* Front right */
 		motor[0] = throttle + out[0] + out[1] + out[2];
 		/* Front left */
@@ -132,22 +141,22 @@ 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) {
-	if (armed)
+	if (status_armed())
 		throttle = t;
 }