xbee: support rc_proto and joystick

Signed-off-by: Olivier Matz <zer0@droids-corp.org>
Signed-off-by: Fabrice Desclaux <serpilliere@droids-corp.org>

diff --git a/commands.c b/commands.c
index a12ed42..be8e03c 100644 (file)
--- a/commands.c
+++ b/commands.c
@@ -35,6 +35,7 @@
 #include <sys/queue.h>
 #include <arpa/inet.h>
 #include <inttypes.h>
+#include <linux/joystick.h>
 
 #include <event.h>
 
@@ -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 (file)
index 0000000..a1e0eba
--- /dev/null
+++ b/joystick.c
@@ -0,0 +1,119 @@
+/*
+ * Copyright (c) 2012, Olivier MATZ <zer0@droids-corp.org>
+ * 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 <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <stdlib.h>
+#include <fcntl.h>
+#include <unistd.h>
+
+#include <linux/input.h>
+#include <linux/joystick.h>
+
+#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 (file)
index 0000000..91b8957
--- /dev/null
+++ b/joystick.h
@@ -0,0 +1,51 @@
+/*
+ * Copyright (c) 2012, Olivier MATZ <zer0@droids-corp.org>
+ * 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 (file)
--- a/main.c
+++ b/main.c
@@ -41,6 +41,7 @@
 #include <time.h>
 #include <netinet/in.h>
 #include <sys/queue.h>
+#include <linux/joystick.h>
 
 #include <getopt.h>
 
@@ -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 (file)
--- 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 (file)
index 0000000..4242477
--- /dev/null
+++ b/rc_proto.c
@@ -0,0 +1,33 @@
+/*
+ * Copyright (c) 2012, Olivier MATZ <zer0@droids-corp.org>
+ * 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 <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+
+#include "rc_proto.h"
+

diff --git a/rc_proto.h b/rc_proto.h
new file mode 100644 (file)
index 0000000..96716d2
--- /dev/null
+++ b/rc_proto.h
@@ -0,0 +1,50 @@
+/*
+ * Copyright (c) 2012, Olivier MATZ <zer0@droids-corp.org>
+ * 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