Below is the file 'stick.c' from this revision. You can also download the file.
/* 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" #include "config.h" #define TWO_PI 6.28318531f #define PI 3.14159265f #define MIN_X config.stick.timing.x.min #define MAX_X config.stick.timing.x.max #define CENTRE_X config.stick.timing.x.centre #define MIN_Y config.stick.timing.y.min #define MAX_Y config.stick.timing.y.max #define CENTRE_Y config.stick.timing.y.centre #define MIN_Z config.stick.timing.z.min #define MAX_Z config.stick.timing.z.max #define CENTRE_Z config.stick.timing.z.centre #define MIN_THR config.stick.timing.throttle.min #define MAX_THR config.stick.timing.throttle.max #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*config.stick.sensitivity.x/360.0 / (MAX_X-CENTRE_X)) #define SCALE_Y (TWO_PI*config.stick.sensitivity.y/360.0 / (MAX_Y-CENTRE_Y)) /* Full scale is a complete rotation in one second */ #define SCALE_Z (TWO_PI*config.stick.sensitivity.z/360.0 / (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++; }