diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/generic.c | 52 | ||||
| -rw-r--r-- | src/main.c | 163 |
2 files changed, 215 insertions, 0 deletions
diff --git a/src/generic.c b/src/generic.c new file mode 100644 index 0000000..bf01e79 --- /dev/null +++ b/src/generic.c @@ -0,0 +1,52 @@ +#include "controller.h" +#include <stdio.h> + +#define CHANNEL_THROTTLE 0 + +#define THROTTLE_MIN 1000 +#define THROTTLE_MAX 2000 + +//convert the value from an axis to an appropriate ppm value +//between 1000 and 2000 +inline channel_t axis_to_channel(int value) { + long centered = (long) value - AXIS_MIN; + long proportional = centered * 1000 / (AXIS_MAX - AXIS_MIN); + return (channel_t) 1000 + 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] = 1500; + channels[2] = 1500; + channels[3] = 1500; + channels[4] = 1500; +} +void event_axis(channel_t* channels, int axis, int value) { + channels[axis+1] = axis_to_channel(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, -10, THROTTLE_MIN, THROTTLE_MAX); + break; + case 5: + channel_step(channels, CHANNEL_THROTTLE, 10, THROTTLE_MIN, THROTTLE_MAX); + break; + case 6: + channel_step(channels, CHANNEL_THROTTLE, -100, THROTTLE_MIN, THROTTLE_MAX); + break; + case 7: + channel_step(channels, CHANNEL_THROTTLE, 100, THROTTLE_MIN, THROTTLE_MAX); + break; + default: + break; + } + } +}
\ No newline at end of file diff --git a/src/main.c b/src/main.c new file mode 100644 index 0000000..6b281cf --- /dev/null +++ b/src/main.c @@ -0,0 +1,163 @@ +#include <stdio.h> +#include <SDL.h> +#include "mavlink/common/mavlink.h" +#include "controller.h" + +struct conf { + + //the joystick id to use by sdl + int joystick_id; + + //maximum delay before the controller's state is printed + int timeout; + + //mav system id of local device + int mav_local_system; + + //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); + + +int main(int argc, char *argv[]) { + + //initialize joystick libraryz + 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 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; + + 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; +} + +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] + ); + + uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); + fwrite(buf, len, 1, stdout); + fflush(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"); + + + /* + mavlink_msg_rc_channels_override_pack(ALL) + fprintf(stdout, "%u\n", channels[0]); + fprintf(stderr, "channel 0: %u\n", channels[0]); + fflush(stdout); + fflush(stderr);*/ +}
\ No newline at end of file |