The unified diff between revisions [056a532c..] and [dc88787e..] 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 [056a532c92301bcb224e1f786c5f6720e8acf3eb] # new_revision [dc88787ecd1d574feba045763baed2a7651ff33d] # # patch "stick.c" # from [50fffafb58abb3d5935c27843ff50c4bd4ec5300] # to [2f04655a185f179c9e8469822e089c308cb3a220] # ============================================================ --- stick.c 50fffafb58abb3d5935c27843ff50c4bd4ec5300 +++ stick.c 2f04655a185f179c9e8469822e089c308cb3a220 @@ -15,24 +15,25 @@ #include "status.h" #include "watchdog.h" #include "log.h" +#include "config.h" #define TWO_PI 6.28318531f #define PI 3.14159265f -#define MIN_X 8720 -#define MAX_X 23800 -#define CENTRE_X 16260 +#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 8720 -#define MAX_Y 23800 -#define CENTRE_Y 16260 +#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 8720 -#define MAX_Z 23800 -#define CENTRE_Z 16300 +#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 9720 -#define MAX_THR 23750 +#define MIN_THR config.stick.timing.throttle.min +#define MAX_THR config.stick.timing.throttle.max #define MIN_REAL_THR 8720 @@ -46,11 +47,11 @@ */ /* 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)) +#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 / (MAX_Z-CENTRE_Z)) +#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)) @@ -59,9 +60,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 +73,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 @@ -152,6 +153,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; @@ -164,9 +169,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) { @@ -177,10 +180,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 /*