The unified diff between revisions [d0420ebd..] and [9142f333..] 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 [d0420ebd87c820e33a32b29727989516e15980a8] # new_revision [9142f3330490a5aa00c1686475633b620c2ef5e7] # # patch "stick.c" # from [b0c0cbffb3863a3992abc1ac1148fc90f019309b] # to [50fffafb58abb3d5935c27843ff50c4bd4ec5300] # ============================================================ --- stick.c b0c0cbffb3863a3992abc1ac1148fc90f019309b +++ stick.c 50fffafb58abb3d5935c27843ff50c4bd4ec5300 @@ -12,30 +12,39 @@ #include "timer.h" #include "trig.h" #include "motor.h" -#include "wmp.h" +#include "status.h" +#include "watchdog.h" +#include "log.h" #define TWO_PI 6.28318531f #define PI 3.14159265f -#define MIN_X 15830 -#define MAX_X 28300 -#define CENTRE_X 22100 +#define MIN_X 8720 +#define MAX_X 23800 +#define CENTRE_X 16260 -#define MIN_Y 18530 -#define MAX_Y 28200 -#define CENTRE_Y 22100 +#define MIN_Y 8720 +#define MAX_Y 23800 +#define CENTRE_Y 16260 -#define MIN_Z 15800 -#define MAX_Z 28304 -#define CENTRE_Z 22100 +#define MIN_Z 8720 +#define MAX_Z 23800 +#define CENTRE_Z 16300 -#define MIN_THR 16500 -#define MAX_THR 28275 +#define MIN_THR 9720 +#define MAX_THR 23750 -#define MIN_REAL_THR 15830 +#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)) @@ -50,8 +59,6 @@ unsigned int stick_counter; unsigned int stick_counter; -bool armed = FALSE; - void stick_update(float x, float y, float z) { float tz = delta_t * z; @@ -77,26 +84,61 @@ void stick_update(float x, float y, floa 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; + unsigned int xi, yi, zi, throttlei; + if (timer_allvalid()) { - x = timer_input(0); - y = timer_input(1); - throttle = timer_input(2); - z = timer_input(3); + xi = timer_input(0); + yi = timer_input(1); + throttlei = timer_input(2); + zi = timer_input(3); - if (!armed) { + 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)) && - (wmp_zero == FALSE)) { - putstr("ARMED!!!\r\n"); - armed = TRUE; - } + (z < (CENTRE_Z + CENTRE_ZONE))) + status_set_ready(STATUS_MODULE_STICK, TRUE); + else + status_set_ready(STATUS_MODULE_STICK,FALSE); } @@ -114,10 +156,13 @@ void stick_input(void) { 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;