You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
195 lines
4.0 KiB
195 lines
4.0 KiB
/* 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++; |
|
} |
|
|
|
|