The unified diff between revisions [d0420ebd..] and [dc88787e..] 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 [dc88787ecd1d574feba045763baed2a7651ff33d]
#
# patch "motor.c"
#  from [500d9a9173fe983ae7c50cf5fac1d37efa96d8ce]
#    to [9f90df05166bf056393d18108f1b25d0fd34da52]
#
============================================================
--- motor.c	500d9a9173fe983ae7c50cf5fac1d37efa96d8ce
+++ motor.c	9f90df05166bf056393d18108f1b25d0fd34da52
@@ -1,15 +1,19 @@
 /* motor.c */
 
 #include "stick.h"
-#include "timer.h"
+#include "thrust.h"
 #include "dcm.h"
 #include "uart.h"
+#include "status.h"
+#include "log.h"
+#include "config.h"
 
 float integral[3] = {0.0f, 0.0f, 0.0f};
 float last[3];
 
 float throttle = 0.0f;
 
+#if 0
 #define Kp 0.2
 #define Ki 0.04
 #define Kd 0.08
@@ -20,34 +24,49 @@ float throttle = 0.0f;
 #define Kd_y 0.00
 #define Ka_y 0.0
 
+#else
 
+#define Kp config.pid.rollpitch.p
+#define Ki config.pid.rollpitch.i
+#define Kd config.pid.rollpitch.d
+#define Ka config.pid.rollpitch.a
+
+#define Kp_y config.pid.yaw.p
+#define Ki_y config.pid.yaw.i
+#define Kd_y config.pid.yaw.d
+#define Ka_y config.pid.yaw.a
+#endif
+
+
 /*
  * Perform a PID loop iteration.
  * roll and pitch are absolute values
  * yaw is, currently, a rate.
+ * For this function only, coordinate convention is:
+ *   x = roll
+ *   y = pitch
+ *   z = yaw
  */
-void motor_pid_update(float troll, float mroll,
-		      float tpitch, float mpitch,
-		      float tyaw, float myaw)
+void motor_pid_update(vec3f target, vec3f measured)
 {
 	float derivative[3];
 	float out[3];
-	float motor[3];
+	float motor[4];
 	float roll, pitch, yaw;
 	float error, max_error;
 	float min_motor;
 	int i;
 
-	roll  = troll  - mroll;
-	pitch = tpitch - mpitch;
-	yaw   = tyaw   - myaw;
+	roll  = target.x - measured.x;
+	pitch = target.y - measured.y;
+	yaw   = target.z - measured.z;
 
 #if 0
         if ((stick_counter % 100) == 0) {
 		putstr("{");
-		putint_s((int)(tyaw * 10000));
+		putint_s((int)(target.z * 10000));
 		putstr(", ");
-		putint_s((int)(myaw * 10000));
+		putint_s((int)(measured.z * 10000));
 		putstr("}\r\n");
 	}
 #endif
@@ -56,20 +75,28 @@ 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;
-	derivative[2] = (-myaw   - last[2]) / delta_t;
+	derivative[0] = (-measured.x - last[0]) / delta_t;
+	derivative[1] = (-measured.y - last[1]) / delta_t;
+	derivative[2] = (-measured.z - last[2]) / delta_t;
 
-	last[0] = -mroll;
-	last[1] = -mpitch;
-	last[2] = -myaw;
+	last[0] = -measured.x;
+	last[1] = -measured.y;
+	last[2] = -measured.z;
 
 	out[0] = roll  * Kp   + integral[0] * Ki   + derivative[0] * Kd;
 	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 +159,27 @@ 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]);
+
+	log_put_uint16((unsigned int) (motor[0] * 65535));
+	log_put_uint16((unsigned int) (motor[1] * 65535));
+	log_put_uint16((unsigned int) (motor[2] * 65535));
+	log_put_uint16((unsigned int) (motor[3] * 65535));
 }
 
 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;
 }