The unified diff between revisions [be147b11..] and [08a35a66..] 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 [be147b11caac304fda1579ac71017eecc3bb79e0] # new_revision [08a35a6680cdf8985cfb16fa6779ee6db7202a9c] # # patch "stick.c" # from [4870f5f016221d25b0d8a4d093fda5dbe0f0b30c] # to [c994ce958c11d747045e0cd4d6879a1fc4d7b4e5] # ============================================================ --- stick.c 4870f5f016221d25b0d8a4d093fda5dbe0f0b30c +++ stick.c c994ce958c11d747045e0cd4d6879a1fc4d7b4e5 @@ -12,37 +12,37 @@ #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 14700 -#define MAX_X 29700 -#define CENTRE_X 22200 +#define MIN_X 8720 +#define MAX_X 23800 +#define CENTRE_X 16260 -#define MIN_Y 14700 -#define MAX_Y 29700 -#define CENTRE_Y 22200 +#define MIN_Y 8720 +#define MAX_Y 23800 +#define CENTRE_Y 16260 -#define MIN_Z 14700 -#define MAX_Z 29700 -#define CENTRE_Z 22200 +#define MIN_Z 8720 +#define MAX_Z 23800 +#define CENTRE_Z 16300 -#define MIN_THR 15700 -#define MAX_THR 29700 +#define MIN_THR 9720 +#define MAX_THR 23750 -#define MIN_REAL_THR 14700 +#define MIN_REAL_THR 8720 #define CENTRE_ZONE 100 -/* With new TX firmware: +/* With new TX firmware, CPPM: * x y thr z - * centre: 22192, 22222, 14687, 22196 - * min: 14686, 14701, 14686, 14687 - * max: 29740, 29740, 29725, 29725 + * 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 */ @@ -59,9 +59,9 @@ unsigned int stick_counter; unsigned int stick_counter; -void stick_update(float x, float y, float z) +void stick_update(vec3f stick) { - float tz = delta_t * z; + float tz = delta_t * stick.z; stick_yaw += tz; @@ -72,16 +72,16 @@ void stick_update(float x, float y, floa stick_yaw -= TWO_PI; #if 0 - z = stick_yaw; + stick.z = stick_yaw; #endif - x = sine(x); - y = sine(y); + stick.x = sine(stick.x); + stick.y = sine(stick.y); #if 0 - z = 1.0/fisqrt(1-x*x-y*y); + stick.z = 1.0/fisqrt(1-stick.x*stick.x-stick.y*stick.y); #endif - dcm_attitude_error(x, y, z); + dcm_attitude_error(stick); } #ifdef STICK_DEBUG_CALIBRATE @@ -105,14 +105,26 @@ void stick_input(void) { 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); + 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 % 20) == 0) + if ((stick_counter % 100) == 0) stick_debug_calibrate(); #endif @@ -140,6 +152,10 @@ void stick_input(void) { 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; @@ -152,9 +168,7 @@ void stick_input(void) { watchdog_kick(WATCHDOG_STICK); /* So the controls are back to front. Let's fix that. */ - x = -x; - y = -y; - z = -z; + vec3f stick = {-x, -y, -z}; #if 0 if ((stick_counter % 100) == 0) { @@ -165,10 +179,10 @@ void stick_input(void) { #endif #if 1 - stick_update(x, y, z); + stick_update(stick); #else if ((stick_counter % 100) == 0) - stick_update(x, y, z); + stick_update(stick); #endif /*