The unified diff between revisions [bfc9e27f..] 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 [bfc9e27f5c40da31ae4269394aaf0545e5856a70]
# new_revision [dc88787ecd1d574feba045763baed2a7651ff33d]
#
# patch "motor.c"
#  from [743afb567c758bd114cb1dd29f25081f3454108c]
#    to [9f90df05166bf056393d18108f1b25d0fd34da52]
#
============================================================
--- motor.c	743afb567c758bd114cb1dd29f25081f3454108c
+++ motor.c	9f90df05166bf056393d18108f1b25d0fd34da52
@@ -6,12 +6,14 @@
 #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
@@ -22,15 +24,30 @@ 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];
@@ -40,16 +57,16 @@ void motor_pid_update(float troll, float
 	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
@@ -67,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;