From 735209cdbe2c6d67c8473e17cdce0ac9496c42d1 Mon Sep 17 00:00:00 2001 From: Olivier Matz Date: Fri, 17 Feb 2012 19:20:08 +0100 Subject: [PATCH] xbee: support rc_proto and joystick Signed-off-by: Olivier Matz Signed-off-by: Fabrice Desclaux --- commands.c | 173 +++++++++++++++++++++++++++++++++++++++++++++++++++++ joystick.c | 119 ++++++++++++++++++++++++++++++++++++ joystick.h | 51 ++++++++++++++++ main.c | 22 ++++++- main.h | 1 + rc_proto.c | 33 ++++++++++ rc_proto.h | 50 ++++++++++++++++ 7 files changed, 448 insertions(+), 1 deletion(-) create mode 100644 joystick.c create mode 100644 joystick.h create mode 100644 rc_proto.c create mode 100644 rc_proto.h diff --git a/commands.c b/commands.c index a12ed42..be8e03c 100644 --- a/commands.c +++ b/commands.c @@ -35,6 +35,7 @@ #include #include #include +#include #include @@ -53,6 +54,8 @@ #include "parse_atcmd.h" #include "parse_neighbor.h" #include "parse_monitor.h" +#include "rc_proto.h" +#include "joystick.h" #include "main.h" static struct monitor_reg_list monitor_list = LIST_HEAD_INITIALIZER(x/*XXX*/); @@ -71,6 +74,10 @@ static struct event range_event; static int range_count = 100; static int range_cur_count = 0; +static struct event rc_send_event; +static int rc_send_period_ms = 100; +static uint64_t rc_send_dstaddr = 0xFFFF; /* broadcast by default */ +static int rc_send_running = 0; static const char *xbee_logfilename = "/tmp/xbee.log"; static void monitor_cb(int s, short event, void *arg) @@ -122,6 +129,27 @@ static void range_cb(int s, short event, void *arg) evtimer_add(&range_event, &tv); } + +static void rc_send_cb(int fd, short event, void *arg) +{ + struct cmdline *cl = arg; + struct timeval tv; + struct rc_proto_channel rc_chan; + int i; + + rc_chan.type = RC_PROTO_TYPE_CHANNEL; + for (i = 0; i< AXIS_NUMBER; i++){ + rc_chan.axis[i] = htons(joyinfo.axis[i]); + } + xbeeapp_send_msg(rc_send_dstaddr, &rc_chan, sizeof(rc_chan), 0); + + evtimer_set(&rc_send_event, rc_send_cb, cl); + tv.tv_sec = 0; + tv.tv_usec = 1000 * rc_send_period_ms; + evtimer_add(&rc_send_event, &tv); +} + + /* ************* */ /* this structure is filled when cmd_stats is parsed successfully */ @@ -610,6 +638,148 @@ cmdline_parse_inst_t cmd_range_dstaddr = { }, }; +/* ************* */ + +/* this structure is filled when cmd_rc_send is parsed successfully */ +struct cmd_rc_send_result { + cmdline_fixed_string_t rc_send; + cmdline_fixed_string_t action; +}; + +/* function called when cmd_rc_send is parsed successfully */ +static void cmd_rc_send_parsed(void *parsed_result, struct cmdline *cl, + void *data) +{ + struct cmd_rc_send_result *res = parsed_result; + + if (!strcmp(res->action, "show")) + ;//XXX + else if (!strcmp(res->action, "start")) { + struct timeval tv; + if (rc_send_running) { + printf("already running\n"); + return; + } + evtimer_set(&rc_send_event, rc_send_cb, cl); + tv.tv_sec = 0; + tv.tv_usec = 0; + evtimer_add(&rc_send_event, &tv); + rc_send_running = 1; + } + else if (!strcmp(res->action, "end")) { + if (rc_send_running == 0) { + printf("not running\n"); + return; + } + rc_send_running = 0; + evtimer_del(&rc_send_event); + } +} + +cmdline_parse_token_string_t cmd_rc_send_rc_send = + TOKEN_STRING_INITIALIZER(struct cmd_rc_send_result, rc_send, "rc_send"); +cmdline_parse_token_string_t cmd_rc_send_action = + TOKEN_STRING_INITIALIZER(struct cmd_rc_send_result, action, + "show#start#end"); + +cmdline_parse_inst_t cmd_rc_send = { + .f = cmd_rc_send_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = "start/stop/show current rc_sending", + .tokens = { /* token list, NULL terminated */ + (void *)&cmd_rc_send_rc_send, + (void *)&cmd_rc_send_action, + NULL, + }, +}; + +/* ************* */ + +/* this structure is filled when cmd_rc_send_period is parsed successfully */ +struct cmd_rc_send_period_result { + cmdline_fixed_string_t rc_send; + cmdline_fixed_string_t action; + uint32_t period; +}; + +/* function called when cmd_rc_send_period is parsed successfully */ +static void cmd_rc_send_period_parsed(void *parsed_result, struct cmdline *cl, + void *data) +{ + struct cmd_rc_send_period_result *res = parsed_result; + + if (res->period < 10) { + printf("error, minimum period is 10 ms\n"); + return; + } + + rc_send_period_ms = res->period; +} + +cmdline_parse_token_string_t cmd_rc_send_period_rc_send_period = + TOKEN_STRING_INITIALIZER(struct cmd_rc_send_period_result, rc_send, + "rc_send"); +cmdline_parse_token_string_t cmd_rc_send_period_action = + TOKEN_STRING_INITIALIZER(struct cmd_rc_send_period_result, action, + "period"); +cmdline_parse_token_num_t cmd_rc_send_period_period = + TOKEN_NUM_INITIALIZER(struct cmd_rc_send_period_result, period, UINT32); + + +cmdline_parse_inst_t cmd_rc_send_period = { + .f = cmd_rc_send_period_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = "set register rc_sending period", + .tokens = { /* token list, NULL terminated */ + (void *)&cmd_rc_send_period_rc_send_period, + (void *)&cmd_rc_send_period_action, + (void *)&cmd_rc_send_period_period, + NULL, + }, +}; + +/* ************* */ + +/* this structure is filled when cmd_rc_send_dstaddr is parsed successfully */ +struct cmd_rc_send_dstaddr_result { + cmdline_fixed_string_t rc_send; + cmdline_fixed_string_t action; + uint64_t dstaddr; +}; + +/* function called when cmd_rc_send_dstaddr is parsed successfully */ +static void cmd_rc_send_dstaddr_parsed(void *parsed_result, struct cmdline *cl, + void *data) +{ + struct cmd_rc_send_dstaddr_result *res = parsed_result; + + rc_send_dstaddr = res->dstaddr; +} + +cmdline_parse_token_string_t cmd_rc_send_dstaddr_rc_send_dstaddr = + TOKEN_STRING_INITIALIZER(struct cmd_rc_send_dstaddr_result, rc_send, + "rc_send"); +cmdline_parse_token_string_t cmd_rc_send_dstaddr_action = + TOKEN_STRING_INITIALIZER(struct cmd_rc_send_dstaddr_result, action, + "dstaddr"); +cmdline_parse_token_num_t cmd_rc_send_dstaddr_dstaddr = + TOKEN_NUM_INITIALIZER(struct cmd_rc_send_dstaddr_result, dstaddr, UINT64); + + +cmdline_parse_inst_t cmd_rc_send_dstaddr = { + .f = cmd_rc_send_dstaddr_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = "set register rc_sending dstaddr", + .tokens = { /* token list, NULL terminated */ + (void *)&cmd_rc_send_dstaddr_rc_send_dstaddr, + (void *)&cmd_rc_send_dstaddr_action, + (void *)&cmd_rc_send_dstaddr_dstaddr, + NULL, + }, +}; + + + /* ************* */ /* this structure is filled when cmd_ping is parsed successfully */ @@ -1315,6 +1485,9 @@ cmdline_parse_ctx_t main_ctx = { &cmd_range_count, &cmd_range_powermask, &cmd_range_dstaddr, + &cmd_rc_send, + &cmd_rc_send_period, + &cmd_rc_send_dstaddr, &cmd_ping, &cmd_raw, &cmd_dump, diff --git a/joystick.c b/joystick.c new file mode 100644 index 0000000..a1e0eba --- /dev/null +++ b/joystick.c @@ -0,0 +1,119 @@ +/* + * Copyright (c) 2012, Olivier MATZ + * All rights reserved. + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the University of California, Berkeley nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "joystick.h" + +int joystick_init(const char *devname, struct joystick_info *joyinfo) +{ + int fd; + + memset(joyinfo, 0, sizeof(*joyinfo)); + joyinfo->nb_axis = 2; + joyinfo->nb_buttons = 2; + joyinfo->version = 0x000800; + strcpy(joyinfo->name, "Unknown"); + + if ((fd = open(devname, O_RDONLY)) < 0) { + perror("jstest"); + return -1; + } + + ioctl(fd, JSIOCGVERSION, &joyinfo->version); + ioctl(fd, JSIOCGAXES, &joyinfo->nb_axis); + ioctl(fd, JSIOCGBUTTONS, &joyinfo->nb_buttons); + ioctl(fd, JSIOCGNAME(JOYSTICK_NAME_LENGTH), joyinfo->name); + ioctl(fd, JSIOCGAXMAP, joyinfo->axmap); + ioctl(fd, JSIOCGBTNMAP, joyinfo->btnmap); + + joyinfo->fd = fd; + return 0; +} + +void joystick_dump(struct joystick_info *joyinfo) +{ + int i; + + if (joyinfo->nb_axis) { + printf("Axes: "); + for (i = 0; i < joyinfo->nb_axis; i++) + printf("%2d:%6d ", i, joyinfo->axis[i]); + } + + if (joyinfo->nb_buttons) { + printf("Buttons: "); + for (i = 0; i < joyinfo->nb_buttons; i++) + printf("%2d:%s ", i, joyinfo->buttons[i] ? "on " : "off"); + } + + printf("\n"); + +} + +void joystick_input(int fd, short event, void *arg) +{ + struct joystick_info *joyinfo; + struct js_event js; + + joyinfo = arg; + + if (read(fd, &js, sizeof(struct js_event)) != sizeof(struct js_event)) { + perror("\njstest: error reading"); + return; + } + + switch(js.type & ~JS_EVENT_INIT) { + case JS_EVENT_BUTTON: + if (js.number >= joyinfo->nb_buttons) { + fprintf(stderr, + "\njstest: button number too high"); + return; + } + joyinfo->buttons[js.number] = js.value; + break; + case JS_EVENT_AXIS: + if (js.number >= joyinfo->nb_axis) { + fprintf(stderr, + "\njstest: axe number too high"); + return; + } + joyinfo->axis[js.number] = js.value; + break; + } +} diff --git a/joystick.h b/joystick.h new file mode 100644 index 0000000..91b8957 --- /dev/null +++ b/joystick.h @@ -0,0 +1,51 @@ +/* + * Copyright (c) 2012, Olivier MATZ + * All rights reserved. + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the University of California, Berkeley nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef JOYSTICK_H +#define JOYSTICK_H + +#define JOYSTICK_NAME_LENGTH 128 +#define JOYSTICK_BUTTON_MAX 0x10 +#define JOYSTICK_AXIS_MAX 0x10 + +struct joystick_info { + int version; + unsigned char nb_axis; + unsigned char nb_buttons; + char buttons[JOYSTICK_BUTTON_MAX]; + char axis[JOYSTICK_AXIS_MAX]; + char name[JOYSTICK_NAME_LENGTH]; + uint16_t btnmap[KEY_MAX - BTN_MISC + 1]; + uint8_t axmap[ABS_MAX + 1]; + int fd; +}; + +int joystick_init(const char *devname, struct joystick_info *joyinfo); +void joystick_input(int fd, short event, void *arg); +void joystick_dump(struct joystick_info *joyinfo); + +#endif diff --git a/main.c b/main.c index 52c0d80..94c0f34 100644 --- a/main.c +++ b/main.c @@ -41,6 +41,7 @@ #include #include #include +#include #include @@ -57,6 +58,7 @@ #include "xbee_buf.h" #include "xbee_proto.h" #include "xbee.h" +#include "joystick.h" #include "main.h" #define TIMEOUT_US 1000000 @@ -75,6 +77,7 @@ struct xbee_dev *xbee_dev; /* events */ static struct event stdin_read_event, xbee_read_event; +static struct event joystick_read_event; static struct cmdline *xbee_cl; @@ -85,6 +88,8 @@ int xbee_raw = 0; int xbee_hexdump = 0; int xbee_debug = 0; FILE *xbee_logfile; +static char *joystick_devname = NULL; +struct joystick_info joyinfo; void xbeeapp_log(int always_on_stdout, const char *fmt, ...) { @@ -696,7 +701,7 @@ parse_args(int argc, char **argv) argvopt = argv; - while ((opt = getopt_long(argc, argvopt, "hd:s:", + while ((opt = getopt_long(argc, argvopt, "hd:s:j:", lgopts, &option_index)) != EOF) { switch (opt) { @@ -718,6 +723,10 @@ parse_args(int argc, char **argv) xbee_raw = 1; break; + case 'j': + joystick_devname = optarg; + break; + /* long options */ case 0: /* if (!strcmp(lgopts[option_index].name, "option")) */ @@ -764,6 +773,17 @@ int main(int argc, char **argv) if (err < 0) return -1; + /* init joystick */ + if (joystick_devname != NULL) { + if (joystick_init(joystick_devname, &joyinfo) < 0) { + fprintf(stderr, "error in joystick init\n"); + return -1; + } + event_set(&joystick_read_event, joyinfo.fd, EV_READ | EV_PERSIST, + joystick_input, &joyinfo); + event_add(&joystick_read_event, NULL); + } + /* open xbee device */ xbee_dev = xbee_open(xbee_devname, xbee_baud); if (xbee_dev == NULL) diff --git a/main.h b/main.h index 548cc62..ec682f4 100644 --- a/main.h +++ b/main.h @@ -47,6 +47,7 @@ extern struct xbee_dev *xbee_dev; extern int xbee_raw; extern int xbee_hexdump; extern int xbee_debug; +extern struct joystick_info joyinfo; extern FILE *xbee_logfile; #define XBEE_LOG_FILE "/home/zer0/xbee.log" diff --git a/rc_proto.c b/rc_proto.c new file mode 100644 index 0000000..4242477 --- /dev/null +++ b/rc_proto.c @@ -0,0 +1,33 @@ +/* + * Copyright (c) 2012, Olivier MATZ + * All rights reserved. + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the University of California, Berkeley nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include + +#include "rc_proto.h" + diff --git a/rc_proto.h b/rc_proto.h new file mode 100644 index 0000000..96716d2 --- /dev/null +++ b/rc_proto.h @@ -0,0 +1,50 @@ +/* + * Copyright (c) 2012, Olivier MATZ + * All rights reserved. + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the University of California, Berkeley nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef RC_PROTO_H +#define RC_PROTO_H + +#define AXIS_NUMBER 4 + +#define RC_PROTO_TYPE_CHANNEL 0 +#define RC_PROTO_TYPE_RANGE 1 + + +/* TODO: Authenticate packet!! */ + +struct rc_proto_channel { + uint8_t type; + int16_t axis[AXIS_NUMBER]; +}; + +struct rc_proto_range { + uint8_t type; + uint8_t power_level; +}; + + +#endif -- 2.39.5