The unified diff between revisions [23a3e9a5..] and [9142f333..] is displayed below. It can also be downloaded as a raw diff.

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

#
# old_revision [23a3e9a50b4034343e3bd217d2c225dcaec064dd]
# new_revision [9142f3330490a5aa00c1686475633b620c2ef5e7]
#
# add_file "stick.c"
#  content [50fffafb58abb3d5935c27843ff50c4bd4ec5300]
#
============================================================
--- /dev/null	
+++ stick.c	50fffafb58abb3d5935c27843ff50c4bd4ec5300
@@ -0,0 +1,192 @@
+/* stick.c */
+
+#ifdef WE_HAVE_SQRT
+#include <math.h>
+#else
+#include "fisqrt.h"
+#endif
+#include "matrix.h"
+#include "stick.h"
+#include "dcm.h"
+#include "uart.h"
+#include "timer.h"
+#include "trig.h"
+#include "motor.h"
+#include "status.h"
+#include "watchdog.h"
+#include "log.h"
+
+#define TWO_PI 6.28318531f
+#define PI 3.14159265f
+
+#define MIN_X 8720
+#define MAX_X 23800
+#define CENTRE_X 16260
+
+#define MIN_Y 8720
+#define MAX_Y 23800
+#define CENTRE_Y 16260
+
+#define MIN_Z 8720
+#define MAX_Z 23800
+#define CENTRE_Z 16300
+
+#define MIN_THR 9720
+#define MAX_THR 23750
+
+#define MIN_REAL_THR 8720
+
+#define CENTRE_ZONE 100
+
+/* With new TX firmware, CPPM:
+ *             x      y    thr      z
+ * centre: 16260, 16258, 16000, 16300
+ * min:     8720,  8718, 8720,  8722 
+ * max:    23790, 23817, 23750, 23803
+ */
+
+/* Full scale is a roll/pitch angle of 30 degrees from the vertical */
+#define SCALE_X (TWO_PI*30.0/360.0 / (MAX_X-CENTRE_X))
+#define SCALE_Y (TWO_PI*30.0/360.0 / (MAX_Y-CENTRE_Y))
+
+/* Full scale is a complete rotation in one second */
+#define SCALE_Z (TWO_PI / (MAX_Z-CENTRE_Z))
+
+/* 0 is min throttle, 1 is max throttle */
+#define SCALE_THROTTLE (1.0f/(MAX_THR - MIN_THR))
+
+float stick_yaw = 0.0f;
+
+unsigned int stick_counter;
+
+void stick_update(float x, float y, float z)
+{
+	float tz = delta_t * z;
+
+	stick_yaw += tz;
+
+	if (stick_yaw <= -PI)
+	    stick_yaw += TWO_PI;
+
+	if (stick_yaw > PI)
+	    stick_yaw -= TWO_PI;
+
+#if 0
+	z = stick_yaw;
+#endif
+
+	x = sine(x);
+	y = sine(y);
+#if 0
+	z = 1.0/fisqrt(1-x*x-y*y);
+#endif
+
+	dcm_attitude_error(x, y, z);
+}
+
+#ifdef STICK_DEBUG_CALIBRATE
+void stick_debug_calibrate()
+{
+	unsigned int t1 = timer_input(0);
+	unsigned int t2 = timer_input(1);
+	unsigned int t3 = timer_input(2);
+	unsigned int t4 = timer_input(3);
+        putstr("S:(");
+        putint(t1);
+        putstr(",");
+        putint(t2);
+        putstr(",");
+        putint(t3);
+        putstr(",");
+        putint(t4);
+        putstr(")\r\n");
+}
+#endif
+
+void stick_input(void) {
+	float x, y, z, throttle;
+	unsigned int xi, yi, zi, throttlei;
+
+	if (timer_allvalid()) {
+		xi = timer_input(0);
+		yi = timer_input(1);
+		throttlei = timer_input(2);
+		zi = timer_input(3);
+
+		log_put_uint16(xi);
+		log_put_uint16(yi);
+		log_put_uint16(throttlei);
+		log_put_uint16(zi);
+
+		x = xi;
+		y = yi;
+		throttle = throttlei;
+		z = zi;
+
+#ifdef STICK_DEBUG_CALIBRATE
+		if ((stick_counter % 100) == 0)
+			stick_debug_calibrate();
+#endif
+
+		if (!status_armed()) {
+			if ((throttle < MIN_THR) &&
+			    (x > (CENTRE_X - CENTRE_ZONE)) &&
+			    (x < (CENTRE_X + CENTRE_ZONE)) &&
+			    (y > (CENTRE_Y - CENTRE_ZONE)) &&
+			    (y < (CENTRE_Y + CENTRE_ZONE)) &&
+			    (z > (CENTRE_Z - CENTRE_ZONE)) &&
+			    (z < (CENTRE_Z + CENTRE_ZONE)))
+				    status_set_ready(STATUS_MODULE_STICK, TRUE);
+			else
+				    status_set_ready(STATUS_MODULE_STICK,FALSE);
+
+		}
+
+		x -= CENTRE_X;
+		y -= CENTRE_Y;
+		z -= CENTRE_Z;
+		x = x * SCALE_X;
+		y = y * SCALE_Y;
+		z = z * SCALE_Z;
+		throttle = (throttle - MIN_THR) * SCALE_THROTTLE;
+		if (throttle < 0.0)
+			throttle = 0.0;
+	} else {
+		x = 0.0f;
+		y = 0.0f;
+		z = 0.0f;
+		throttle = 0.0f;
+		status_set_ready(STATUS_MODULE_STICK,FALSE);
+	}
+
+	motor_set_throttle(throttle);
+
+	watchdog_kick(WATCHDOG_STICK);
+
+	/* So the controls are back to front. Let's fix that. */
+	x = -x;
+	y = -y;
+	z = -z;
+
+#if 0
+	if ((stick_counter % 100) == 0) {
+		putstr("[");
+		putint_s((int)(z * 10000));
+		putstr("] ");
+	}
+#endif
+
+#if 1
+	stick_update(x, y, z);
+#else
+	if ((stick_counter % 100) == 0)
+		stick_update(x, y, z);
+#endif
+
+/*
+	if ((stick_counter & 3) == 0)
+		stick_send_packet();
+*/
+	stick_counter++;
+}
+