From 06493e2a9a78aeebe7d6d118b883885445cf7571 Mon Sep 17 00:00:00 2001 From: Jakob Odersky Date: Wed, 24 Jun 2015 14:08:50 +0200 Subject: make controllers more modular --- Makefile | 7 +- src/controller.h | 29 +++++ src/controllers/generic.c | 91 +++++++++++++++ src/controllers/generic.h | 8 ++ src/generic.c | 63 ---------- src/include/controller.h | 38 ------ src/main.c | 291 ++++++++++++++++++++++++---------------------- 7 files changed, 286 insertions(+), 241 deletions(-) create mode 100644 src/controller.h create mode 100644 src/controllers/generic.c create mode 100644 src/controllers/generic.h delete mode 100644 src/generic.c delete mode 100644 src/include/controller.h diff --git a/Makefile b/Makefile index c532dd3..327ac2d 100644 --- a/Makefile +++ b/Makefile @@ -6,9 +6,10 @@ CFLAGS=-std=gnu99 -Wall -O2 LD=gcc LDFLAGS=-O2 -SOURCEDIRS=src +SOURCEDIRS=src src/controllers INCLUDEDIRS=\ - src/include\ + src \ + src/controllers \ ext/mavlink/include SOURCES=$(foreach dir,$(SOURCEDIRS),$(wildcard $(dir)/*.c)) @@ -31,4 +32,4 @@ clean: rm -f $$dir/*.o; \ done rm -f $(TARGET) - rm -f $(TARGET)-static \ No newline at end of file + rm -f $(TARGET)-static diff --git a/src/controller.h b/src/controller.h new file mode 100644 index 0000000..4712dcc --- /dev/null +++ b/src/controller.h @@ -0,0 +1,29 @@ +#ifndef CONTROLLER_H +#define CONTROLLER_H + +#include +#include +#include + +#define CHANNEL_UNUSED 65535 +#define CHANNELS 8 + +#define AXIS_MIN -32768 +#define AXIS_MAX 32767 + +typedef uint16_t channel_t; + +struct controller { + void (*reset)(struct controller* cont); + void (*axis)(struct controller* cont, int axis_id, int value); + void (*button)(struct controller* cont, int button_id, bool down); + channel_t channels[CHANNELS]; +}; + +inline channel_t channel_clamp(channel_t channel, channel_t min, channel_t max) +{ + channel_t upper = (channel < max) ? channel : max; + return (min < upper) ? upper : min; +} + +#endif diff --git a/src/controllers/generic.c b/src/controllers/generic.c new file mode 100644 index 0000000..04044b0 --- /dev/null +++ b/src/controllers/generic.c @@ -0,0 +1,91 @@ +#include "generic.h" +#include + +#define CHANNEL_THROTTLE 0 +#define CHANNEL_ATTITUDE_OFFSET 1 + +#define THROTTLE_MIN 1000 +#define THROTTLE_MAX 2000 +#define THROTTLE_STEP_SMALL 10 +#define THROTTLE_STEP_LARGE 100 + +#define ATTITUDE_MIN 1000 +#define ATTITUDE_MAX 2000 + +/* + * convert the value from an axis to an appropriate ppm value + * between 1000 and 2000 + */ +inline channel_t axis_to_attitude(int value) +{ + long centered = (long) value - AXIS_MIN; + long proportional = centered * ATTITUDE_MIN / (AXIS_MAX - AXIS_MIN); + return (channel_t) ATTITUDE_MAX - ATTITUDE_MIN + proportional; +} + +inline void channel_step(channel_t* channels, size_t channel_index, char step, channel_t min, channel_t max) +{ + int current = (int) channels[channel_index]; + int next = current + step; + if (next > (int) max) { + next = max; + } + if (next < (int) min) { + next = min; + } + channels[channel_index] = (channel_t) next; +} + +static void reset(struct controller* controller) +{ + channel_t *channels = controller->channels; + + for (size_t i = 0; i < CHANNELS; ++i) { + channels[i] = CHANNEL_UNUSED; + } + + channels[0] = THROTTLE_MIN; + channels[1] = ATTITUDE_MIN; + channels[2] = ATTITUDE_MIN; + channels[3] = ATTITUDE_MIN; + channels[4] = ATTITUDE_MIN; +} + +static void axis(struct controller* controller, int axis_id, int value) +{ + controller->channels[CHANNEL_ATTITUDE_OFFSET+axis_id] = axis_to_attitude(value); +} + +static void button(struct controller* controller, int button_id, bool down) +{ + if (!down){ + return; + } + + switch(button_id) { + case 0: + reset(controller); + break; + case 4: + channel_step(controller->channels, CHANNEL_THROTTLE, -THROTTLE_STEP_SMALL, THROTTLE_MIN, THROTTLE_MAX); + break; + case 5: + channel_step(controller->channels, CHANNEL_THROTTLE, THROTTLE_STEP_SMALL, THROTTLE_MIN, THROTTLE_MAX); + break; + case 6: + channel_step(controller->channels, CHANNEL_THROTTLE, -THROTTLE_STEP_LARGE, THROTTLE_MIN, THROTTLE_MAX); + break; + case 7: + channel_step(controller->channels, CHANNEL_THROTTLE, THROTTLE_STEP_LARGE, THROTTLE_MIN, THROTTLE_MAX); + break; + default: + fprintf(stderr, "Button number %d unsupported.\n", button_id); + break; + } +} + +struct controller controller_generic = { + .reset = reset, + .axis = axis, + .button = button +}; diff --git a/src/controllers/generic.h b/src/controllers/generic.h new file mode 100644 index 0000000..f5566e1 --- /dev/null +++ b/src/controllers/generic.h @@ -0,0 +1,8 @@ +#ifndef CONTROLLERS_GENERIC_H +#define CONTROLLERS_GENERIC_H + +#include + +struct controller controller_generic; + +#endif diff --git a/src/generic.c b/src/generic.c deleted file mode 100644 index 683598b..0000000 --- a/src/generic.c +++ /dev/null @@ -1,63 +0,0 @@ -#include "controller.h" -#include - -#define CHANNEL_THROTTLE 0 -#define CHANNEL_ATTITUDE_OFFSET 1 - -#define THROTTLE_MIN 1000 -#define THROTTLE_MAX 2000 -#define THROTTLE_STEP_SMALL 10 -#define THROTTLE_STEP_LARGE 100 - -#define ATTITUDE_MIN 1000 -#define ATTITUDE_MAX 2000 - -/* - * convert the value from an axis to an appropriate ppm value - * between 1000 and 2000 - */ -inline channel_t axis_to_attitude(int value) { - long centered = (long) value - AXIS_MIN; - long proportional = centered * ATTITUDE_MIN / (AXIS_MAX - AXIS_MIN); - return (channel_t) ATTITUDE_MAX - ATTITUDE_MIN + proportional; -} - -void channel_reset(channel_t* channels) { - for (size_t i = 0; i < CHANNELS; ++i) { - channels[i] = CHANNEL_UNUSED; - } - channels[0] = THROTTLE_MIN; - channels[1] = ATTITUDE_MIN; - channels[2] = ATTITUDE_MIN; - channels[3] = ATTITUDE_MIN; - channels[4] = ATTITUDE_MIN; -} - -void event_axis(channel_t* channels, int axis, int value) { - channels[CHANNEL_ATTITUDE_OFFSET+axis] = axis_to_attitude(value); -} - -void event_button(channel_t* channels, int button, bool value) { - if (value) { - switch(button) { - case 0: - channel_reset(channels); - break; - case 4: - channel_step(channels, CHANNEL_THROTTLE, -THROTTLE_STEP_SMALL, THROTTLE_MIN, THROTTLE_MAX); - break; - case 5: - channel_step(channels, CHANNEL_THROTTLE, THROTTLE_STEP_SMALL, THROTTLE_MIN, THROTTLE_MAX); - break; - case 6: - channel_step(channels, CHANNEL_THROTTLE, -THROTTLE_STEP_LARGE, THROTTLE_MIN, THROTTLE_MAX); - break; - case 7: - channel_step(channels, CHANNEL_THROTTLE, THROTTLE_STEP_LARGE, THROTTLE_MIN, THROTTLE_MAX); - break; - default: - printf("button unsupported %d\n", button); - break; - } - } -} \ No newline at end of file diff --git a/src/include/controller.h b/src/include/controller.h deleted file mode 100644 index a74d035..0000000 --- a/src/include/controller.h +++ /dev/null @@ -1,38 +0,0 @@ -#ifndef CONTROLLER_H -#define CONTROLLER_H - -#include -#include -#include - -#define AXIS_MIN -32768 -#define AXIS_MAX 32767 - -#define CHANNEL_UNUSED 65535 -#define CHANNELS 8 - -typedef uint16_t channel_t; - -inline channel_t channel_clamp(channel_t channel, channel_t min, channel_t max) { - channel_t upper = (channel < max) ? channel : max; - return (min < upper) ? upper : min; -} - -inline void channel_step(channel_t* channels, size_t channel_index, char step, channel_t min, channel_t max) { - int current = (int) channels[channel_index]; - int next = current + step; - if (next > (int) max) { - next = max; - } - if (next < (int) min) { - next = min; - } - channels[channel_index] = (channel_t) next; -} - -void channel_reset(channel_t* channels); -void event_axis(channel_t* channels, int axis, int value); -void event_button(channel_t* channels, int button, bool value); - - -#endif \ No newline at end of file diff --git a/src/main.c b/src/main.c index adb62d1..29c1240 100644 --- a/src/main.c +++ b/src/main.c @@ -3,156 +3,173 @@ #include #include "mavlink/common/mavlink.h" #include "controller.h" +#include "generic.h" struct conf { - //the joystick id to use by sdl - int joystick_id; + //the joystick id to use by sdl + int joystick_id; - //maximum delay before the controller's state is printed - int timeout; + //maximum delay before the controller's state is printed [ms] + int timeout; - //mav system id of local device - int mav_local_system; + //mav system id of local device + int mav_local_system; - //mav component id of local device - int mav_local_component; + //mav component id of local device + int mav_local_component; - //mav system id of target device - int mav_target_system; - -}; - -static int run(const struct conf* conf); -static void mav_out(uint8_t system_id, uint8_t component_id, uint8_t rsystem_id, channel_t* channels); + //mav system id of target device + int mav_target_system; + //the controller to use + struct controller* controller; -int main(int argc, char *argv[]) { - - //initialize joystick library - if (SDL_InitSubSystem(SDL_INIT_JOYSTICK) < 0) { - fprintf(stderr, "Couldn't initialize SDL joystick subsystem: %s\n", SDL_GetError()); - exit(1); - } - - struct conf conf = { - .joystick_id = 0, - .timeout = 500, - .mav_local_system = 42, - .mav_local_component = 22, - .mav_target_system = 0 - }; +}; - return run(&conf); +static void print_mav(uint8_t system_id, uint8_t component_id, uint8_t rsystem_id, channel_t* channels); + +static SDL_Joystick* joystick_open(int joystick_id) +{ + SDL_Joystick* joystick = SDL_JoystickOpen(joystick_id); + if(joystick) { + fprintf(stderr, "Opened joystick number %d\n", joystick_id); + fprintf(stderr, "Name: %s\n", SDL_JoystickName(joystick)); + fprintf(stderr, "Number of axes: %d\n", SDL_JoystickNumAxes(joystick)); + fprintf(stderr, "Number of buttons: %d\n", SDL_JoystickNumButtons(joystick)); + fprintf(stderr, "Number of balls: %d\n", SDL_JoystickNumBalls(joystick)); + return joystick; + } else { + fprintf(stderr, "Couldn't open joystick number %d.\n", joystick_id); + return NULL; + } } -static int run(const struct conf* conf) { - - channel_t channels[CHANNELS]; - channel_reset(channels); - - int joystick_id = conf->joystick_id; - - //pointer to joystick in use - SDL_Joystick* joystick = NULL; - - //event loop - SDL_Event e; - bool cont = true; - while (cont) { - - if(SDL_WaitEventTimeout(&e, conf->timeout)) { - switch( e.type ) { - - case SDL_JOYDEVICEADDED: - if (e.jdevice.which == joystick_id) { - fprintf(stderr, "Joystick %d connected\n", e.jdevice.which); - joystick=SDL_JoystickOpen(e.jdevice.which); - if(joystick) { - fprintf(stderr, "Opened joystick %d\n", e.jdevice.which); - fprintf(stderr, "Name: %s\n", SDL_JoystickName(joystick)); - fprintf(stderr, "Number of axes: %d\n", SDL_JoystickNumAxes(joystick)); - fprintf(stderr, "Number of buttons: %d\n", SDL_JoystickNumButtons(joystick)); - fprintf(stderr, "Number of balls: %d\n", SDL_JoystickNumBalls(joystick)); - } else { - fprintf(stderr, "Couldn't open joystick %d\n", e.jdevice.which); - } - } - break; - - case SDL_JOYDEVICEREMOVED: - if (e.jdevice.which == joystick_id) { - if (SDL_JoystickGetAttached(joystick)) { - SDL_JoystickClose(joystick); - } - fprintf(stderr, "Joystick %d disconnected\n", e.jdevice.which); - } - break; - - case SDL_JOYAXISMOTION: - event_axis(channels, e.jaxis.axis, e.jaxis.value); - break; - - case SDL_JOYBUTTONDOWN: - event_button(channels, e.jbutton.button, true); - break; +static int run(const struct conf* conf) +{ + struct controller* cont = conf->controller; + cont->reset(cont); + + int joystick_id = conf->joystick_id; + + //initialize joystick library + if (SDL_InitSubSystem(SDL_INIT_JOYSTICK) < 0) { + fprintf(stderr, "Couldn't initialize SDL joystick subsystem: %s\n", SDL_GetError()); + return -1; + } + + //pointer to joystick in use + SDL_Joystick* joystick = joystick_open(joystick_id); + if (joystick == NULL) { + return -1; + } + + //event loop + SDL_Event e; + bool quit = false; + while (!quit) { + + if(SDL_WaitEventTimeout(&e, conf->timeout)) { + switch (e.type) { + case SDL_JOYDEVICEADDED: + fprintf(stderr, "Joystick number %d connected\n", e.jdevice.which); + if (e.jdevice.which == joystick_id) { + joystick = joystick_open(joystick_id); + } else { + fprintf(stderr, "Wrong id. Found %d, expecting %d.\n", e.jdevice.which, joystick_id); + } + break; + + case SDL_JOYDEVICEREMOVED: + if (e.jdevice.which == joystick_id) { + if (SDL_JoystickGetAttached(joystick)) { + SDL_JoystickClose(joystick); + } + fprintf(stderr, "Joystick %d disconnected\n", e.jdevice.which); + } + break; + + case SDL_JOYAXISMOTION: + cont->axis(cont, e.jaxis.axis, e.jaxis.value); + break; + + case SDL_JOYBUTTONDOWN: + cont->button(cont, e.jbutton.button, true); + break; - case SDL_JOYBUTTONUP: - event_button(channels, e.jbutton.button, false); - break; - - case SDL_QUIT: - cont = false; - break; - - default: - fprintf(stderr,"Unsupported event type: %d\n", e.type); - break; - } - - } - mav_out( - conf->mav_local_system, - conf->mav_local_component, - conf->mav_target_system, - channels - ); - } - return 0; + case SDL_JOYBUTTONUP: + cont->button(cont, e.jbutton.button, false); + break; + + case SDL_QUIT: + quit = true; + break; + + default: + fprintf(stderr,"Unsupported event type: %d\n", e.type); + break; + } + } + + print_mav( + conf->mav_local_system, + conf->mav_local_component, + conf->mav_target_system, + cont->channels + ); + } + return 0; } -static void mav_out(uint8_t system_id, uint8_t component_id, uint8_t rsystem_id, channel_t* channels) { - - mavlink_message_t msg; - uint8_t buf[MAVLINK_MAX_PACKET_LEN]; - - mavlink_msg_rc_channels_override_pack( - system_id, - component_id, - &msg, - rsystem_id, - MAV_COMP_ID_ALL, - channels[0], - channels[1], - channels[2], - channels[3], - channels[4], - channels[5], - channels[6], - channels[7] - ); +static void print_mav(uint8_t system_id, uint8_t component_id, uint8_t rsystem_id, channel_t* channels) +{ + + mavlink_message_t msg; + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; + + mavlink_msg_rc_channels_override_pack( + system_id, + component_id, + &msg, + rsystem_id, + MAV_COMP_ID_ALL, + channels[0], + channels[1], + channels[2], + channels[3], + channels[4], + channels[5], + channels[6], + channels[7] + ); - uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); - fwrite(buf, len, 1, stdout); - fflush(stdout); - fsync(fileno(stdout)); - - fprintf(stderr, "local system id: \t%u\n", system_id); - fprintf(stderr, "local component id: \t%u\n", component_id); - fprintf(stderr, "remote system id: \t%u\n", rsystem_id); - fprintf(stderr, "remote component id: \t%u\n", MAV_COMP_ID_ALL); - for (size_t i = 0; i < CHANNELS; ++i) { - fprintf(stderr, "channel %zu: \t\t%u\n", i, channels[i]); - } - fprintf(stderr, "\n"); -} \ No newline at end of file + uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); + fwrite(buf, len, 1, stdout); + fflush(stdout); + fsync(fileno(stdout)); + + fprintf(stderr, "local system id: \t%u\n", system_id); + fprintf(stderr, "local component id: \t%u\n", component_id); + fprintf(stderr, "remote system id: \t%u\n", rsystem_id); + fprintf(stderr, "remote component id: \t%u\n", MAV_COMP_ID_ALL); + for (size_t i = 0; i < CHANNELS; ++i) { + fprintf(stderr, "channel %zu: \t\t%u\n", i, channels[i]); + } + fprintf(stderr, "\n"); +} + + +int main(int argc, char *argv[]) +{ + struct conf conf = { + .joystick_id = 0, + .timeout = 500, + .mav_local_system = 42, + .mav_local_component = 22, + .mav_target_system = 0, + .controller = &controller_generic + }; + + + return run(&conf); +} -- cgit v1.2.3