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: 'stick.c'

#
# old_revision [23a3e9a50b4034343e3bd217d2c225dcaec064dd]
# new_revision [24d5b9f4dff9135787b198fe1127d9c1e3326b9c]
#
# add_file "stick.c"
#  content [ad24bc7d9d8c95c497a1b5bd7a13d8e1619ca539]
#
============================================================
--- /dev/null	
+++ stick.c	ad24bc7d9d8c95c497a1b5bd7a13d8e1619ca539
@@ -0,0 +1,149 @@
+/* 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 "wmp.h"
+#include "status.h"
+#include "watchdog.h"
+
+#define TWO_PI 6.28318531f
+#define PI 3.14159265f
+
+#define MIN_X 15830
+#define MAX_X 28300
+#define CENTRE_X 22100
+
+#define MIN_Y 18530
+#define MAX_Y 28200
+#define CENTRE_Y 22100
+
+#define MIN_Z 15800
+#define MAX_Z 28304
+#define CENTRE_Z 22100
+
+#define MIN_THR 16500
+#define MAX_THR 28275
+
+#define MIN_REAL_THR 15830
+
+#define CENTRE_ZONE 100
+
+/* 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);
+}
+
+void stick_input(void) {
+	float x, y, z, throttle;
+	if (timer_allvalid()) {
+		x = timer_input(0);
+		y = timer_input(1);
+		throttle = timer_input(2);
+		z = timer_input(3);
+
+		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++;
+}
+