The unified diff between revisions [23a3e9a5..] and [08a35a66..] 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 [08a35a6680cdf8985cfb16fa6779ee6db7202a9c] # # add_file "stick.c" # content [c994ce958c11d747045e0cd4d6879a1fc4d7b4e5] # ============================================================ --- /dev/null +++ stick.c c994ce958c11d747045e0cd4d6879a1fc4d7b4e5 @@ -0,0 +1,194 @@ +/* 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(vec3f stick) +{ + float tz = delta_t * stick.z; + + stick_yaw += tz; + + if (stick_yaw <= -PI) + stick_yaw += TWO_PI; + + if (stick_yaw > PI) + stick_yaw -= TWO_PI; + +#if 0 + stick.z = stick_yaw; +#endif + + stick.x = sine(stick.x); + stick.y = sine(stick.y); +#if 0 + stick.z = 1.0/fisqrt(1-stick.x*stick.x-stick.y*stick.y); +#endif + + dcm_attitude_error(stick); +} + +#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 { + log_put_uint16(0); + log_put_uint16(0); + log_put_uint16(0); + log_put_uint16(0); + 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. */ + vec3f stick = {-x, -y, -z}; + +#if 0 + if ((stick_counter % 100) == 0) { + putstr("["); + putint_s((int)(z * 10000)); + putstr("] "); + } +#endif + +#if 1 + stick_update(stick); +#else + if ((stick_counter % 100) == 0) + stick_update(stick); +#endif + +/* + if ((stick_counter & 3) == 0) + stick_send_packet(); +*/ + stick_counter++; +} +