|
|
|
@ -19,25 +19,32 @@
|
|
|
|
|
#define TWO_PI 6.28318531f |
|
|
|
|
#define PI 3.14159265f |
|
|
|
|
|
|
|
|
|
#define MIN_X 15830 |
|
|
|
|
#define MAX_X 28300 |
|
|
|
|
#define CENTRE_X 22100 |
|
|
|
|
#define MIN_X 14700 |
|
|
|
|
#define MAX_X 29700 |
|
|
|
|
#define CENTRE_X 22200 |
|
|
|
|
|
|
|
|
|
#define MIN_Y 18530 |
|
|
|
|
#define MAX_Y 28200 |
|
|
|
|
#define CENTRE_Y 22100 |
|
|
|
|
#define MIN_Y 14700 |
|
|
|
|
#define MAX_Y 29700 |
|
|
|
|
#define CENTRE_Y 22200 |
|
|
|
|
|
|
|
|
|
#define MIN_Z 15800 |
|
|
|
|
#define MAX_Z 28304 |
|
|
|
|
#define CENTRE_Z 22100 |
|
|
|
|
#define MIN_Z 14700 |
|
|
|
|
#define MAX_Z 29700 |
|
|
|
|
#define CENTRE_Z 22200 |
|
|
|
|
|
|
|
|
|
#define MIN_THR 16500 |
|
|
|
|
#define MAX_THR 28275 |
|
|
|
|
#define MIN_THR 15700 |
|
|
|
|
#define MAX_THR 29700 |
|
|
|
|
|
|
|
|
|
#define MIN_REAL_THR 15830 |
|
|
|
|
#define MIN_REAL_THR 14700 |
|
|
|
|
|
|
|
|
|
#define CENTRE_ZONE 100 |
|
|
|
|
|
|
|
|
|
/* With new TX firmware:
|
|
|
|
|
* x y thr z |
|
|
|
|
* centre: 22192, 22222, 14687, 22196 |
|
|
|
|
* min: 14686, 14701, 14686, 14687 |
|
|
|
|
* max: 29740, 29740, 29725, 29725 |
|
|
|
|
*/ |
|
|
|
|
|
|
|
|
|
/* 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)) |
|
|
|
@ -77,6 +84,25 @@ void stick_update(float x, float y, float z)
|
|
|
|
|
dcm_attitude_error(x, y, z); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
#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; |
|
|
|
|
if (timer_allvalid()) { |
|
|
|
@ -85,6 +111,11 @@ void stick_input(void) {
|
|
|
|
|
throttle = timer_input(2); |
|
|
|
|
z = timer_input(3); |
|
|
|
|
|
|
|
|
|
#ifdef STICK_DEBUG_CALIBRATE |
|
|
|
|
if ((stick_counter % 20) == 0) |
|
|
|
|
stick_debug_calibrate(); |
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
if (!status_armed()) { |
|
|
|
|
if ((throttle < MIN_THR) && |
|
|
|
|
(x > (CENTRE_X - CENTRE_ZONE)) && |
|
|
|
|