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.
149 lines
2.7 KiB
149 lines
2.7 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 "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++; |
|
} |
|
|
|
|