The unified diff between revisions [23a3e9a5..] 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 [23a3e9a50b4034343e3bd217d2c225dcaec064dd]
# new_revision [24d5b9f4dff9135787b198fe1127d9c1e3326b9c]
#
# add_file "motor.c"
#  content [0a5cff753a89545c1d3f23027cfb4f651db669cd]
#
============================================================
--- /dev/null	
+++ motor.c	0a5cff753a89545c1d3f23027cfb4f651db669cd
@@ -0,0 +1,162 @@
+/* motor.c */
+
+#include "stick.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.3
+#define Ki 0.02
+#define Kd 0.08
+#define Ka 0.0
+
+#define Kp_y 0.2
+#define Ki_y 0.00
+#define Kd_y 0.00
+#define Ka_y 0.0
+
+
+/*
+ * Perform a PID loop iteration.
+ * roll and pitch are absolute values
+ * yaw is, currently, a rate.
+ */
+void motor_pid_update(float troll, float mroll,
+		      float tpitch, float mpitch,
+		      float tyaw, float myaw)
+{
+	float derivative[3];
+	float out[3];
+	float motor[3];
+	float roll, pitch, yaw;
+	float error, max_error;
+	float min_motor;
+	int i;
+
+	roll  = troll  - mroll;
+	pitch = tpitch - mpitch;
+	yaw   = tyaw   - myaw;
+
+#if 0
+        if ((stick_counter % 100) == 0) {
+		putstr("{");
+		putint_s((int)(tyaw * 10000));
+		putstr(", ");
+		putint_s((int)(myaw * 10000));
+		putstr("}\r\n");
+	}
+#endif
+
+	integral[0] += roll * delta_t;
+	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;
+
+	last[0] = -mroll;
+	last[1] = -mpitch;
+	last[2] = -myaw;
+
+	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 (status_armed()) {
+		/* Front right */
+		motor[0] = throttle + out[0] + out[1] + out[2];
+		/* Front left */
+		motor[1] = throttle - out[0] + out[1] - out[2];
+		/* Rear left */
+		motor[2] = throttle - out[0] - out[1] + out[2];
+		/* Rear right */
+		motor[3] = throttle + out[0] - out[1] - out[2];
+	} else {
+		motor[0] = 0.0;
+		motor[1] = 0.0;
+		motor[2] = 0.0;
+		motor[3] = 0.0;
+	}
+
+	max_error = 0.0;
+	min_motor = 1.0;
+	for (i = 0; i < 3; i++) {
+		if (motor[i] < 0.0)
+			motor[i] = 0.0;
+		if (motor[i] > 1.0f) {
+			error = motor[i] - 1.0f;
+			if (error > max_error)
+				max_error = error;
+		}
+		if (motor[i] < min_motor)
+			min_motor = motor[i];
+	}
+
+	if (max_error > 0.0) {
+		for (i = 0; i < 3; i++) {
+			motor[i] -= max_error;
+			if (motor[i] < 0.0)
+				motor[i] = 0.0;
+		}
+	}
+
+	if (throttle <= 0.0) {
+		motor[0] = 0.0;
+		motor[1] = 0.0;
+		motor[2] = 0.0;
+		motor[3] = 0.0;
+		integral[0] = 0.0;
+		integral[1] = 0.0;
+		integral[2] = 0.0;
+	}
+	if (max_error < min_motor) {
+		float new_throttle2, new_out[3];
+		new_throttle2 = (motor[0] + motor[1] + motor[2] + motor[3])/2.0;
+		new_out[0] = (motor[0] + motor[3] - new_throttle2)/2.0;
+		new_out[1] = (motor[0] + motor[1] - new_throttle2)/2.0;
+		new_out[2] = (motor[0] + motor[2] - new_throttle2)/2.0;
+
+		/* Anti-windup */
+		for (i = 0; i < 3; i++) {
+			if (new_out[i] > 1.0)
+				integral[i] -= (new_out[i]-1.0) * Ka;
+			if (new_out[i] < 0.0)
+				integral[i] -= (new_out[i]) * Ka;
+		}
+	}
+
+	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;
+	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 (status_armed())
+		throttle = t;
+}
+