The unified diff between revisions [4f22e7ef..] 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 [4f22e7ef7d3064e3b51a5b868a4722f3f13c747b]
# new_revision [dc88787ecd1d574feba045763baed2a7651ff33d]
#
# patch "motor.c"
#  from [3772a98b596df907c16e491694c952392f804893]
#    to [9f90df05166bf056393d18108f1b25d0fd34da52]
#
============================================================
--- motor.c	3772a98b596df907c16e491694c952392f804893
+++ motor.c	9f90df05166bf056393d18108f1b25d0fd34da52
@@ -5,12 +5,15 @@
 #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
@@ -21,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
@@ -66,13 +84,13 @@ void motor_pid_update(float troll, float
 	}
 
 	/* 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;
@@ -145,6 +163,11 @@ void motor_pid_update(float troll, float
 	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) {