rc_proto: choose best power level for rc transmit

diff --git a/commands.c b/commands.c
index 1489cb586e9159594fe38b9a9dd6d031f91a1013..9e000bd978b41bf81c63e731e7a064694051c20e 100644 (file)
--- a/commands.c
+++ b/commands.c
@@ -76,7 +76,6 @@ 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";
 
@@ -134,14 +133,8 @@ 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);
+       rc_proto_send_channels();
 
        evtimer_set(&rc_send_event, rc_send_cb, cl);
        tv.tv_sec = 0;

diff --git a/main.c b/main.c
index 4847d11087363d4c5d24a7a57fca3cb30683877b..fd658a0657d7ff6e1c66606be259859bfcb0aa51 100644 (file)
--- a/main.c
+++ b/main.c
@@ -59,6 +59,7 @@
 #include "xbee_proto.h"
 #include "xbee.h"
 #include "joystick.h"
+#include "rc_proto.h"
 #include "main.h"
 
 #define TIMEOUT_US 1000000
@@ -260,6 +261,41 @@ static int dump_atcmd(struct xbee_ctx *ctx, struct xbee_atresp_hdr *frame,
        return 0;
 }
 
+int xbee_recv_data(struct xbee_recv_hdr *recvframe, unsigned len)
+{
+       int datalen = len - sizeof(*recvframe);
+       struct rc_proto_hdr *rch = (struct rc_proto_hdr *) &recvframe->data;
+
+       if (datalen < sizeof(struct rc_proto_hdr))
+               return -1;
+
+       switch (rch->type) {
+               case RC_PROTO_TYPE_CHANNEL:
+                       if (datalen != sizeof(struct rc_proto_channel))
+                               return -1;
+                       break;
+               case RC_PROTO_TYPE_RANGE: {
+                       struct rc_proto_range *rcr =
+                               (struct rc_proto_range *) recvframe->data;
+
+                       if (datalen != sizeof(struct rc_proto_range))
+                               return -1;
+
+                       if (rcr->power_level >= MAX_POWER_LEVEL)
+                               return -1;
+
+                       rc_proto_rx_range(rcr->power_level);
+
+                       break;
+               }
+               default:
+                       return -1;
+       }
+
+       return 0;
+}
+
+
 void xbee_rx(struct xbee_dev *dev, int channel, int type,
             void *frame, unsigned len, void *opaque)
 {
@@ -326,19 +362,8 @@ void xbee_rx(struct xbee_dev *dev, int channel, int type,
                }
 
                case XBEE_TYPE_RECV: {
-                       struct xbee_recv_hdr *recvframe = frame;
-                       int recvlen = len - sizeof(*recvframe);
-                       int on_stdout = 1;
-
-                       /* if we receive a range-test frame, ask for RSSI now */
-                       if (recvlen >= strlen("range") &&
-                           !strncmp((char *)recvframe->data,
-                                    "range", strlen("range"))) {
-                               xbeeapp_send_atcmd("DB", NULL, 0, 0, NULL, NULL);
-                               on_stdout = 0;
-                       }
-                       hexdump(on_stdout, "rx data", recvframe->data,
-                               recvlen);
+                       if (xbee_recv_data(frame, len) < 0)
+                               do_hexdump = 1;
                        break;
                }
 
@@ -781,6 +806,9 @@ int main(int argc, char **argv)
        if (err < 0)
                return -1;
 
+       /* init rc_proto */
+       rc_proto_init();
+
        /* init joystick */
        if (joystick_devname != NULL) {
                if (joystick_init(joystick_devname, &joyinfo) < 0) {

diff --git a/main.h b/main.h
index 693a5c8081fb4a47710e3def260da293605025fe..88629df2811ff3ccf68487d7a39ca84dc7c9247d 100644 (file)
--- a/main.h
+++ b/main.h
@@ -32,6 +32,8 @@ enum xbee_ctx_type {
        DUMP_CONF,
 };
 
+#define MAX_POWER_LEVEL 5
+
 /* used for timeouts and xbee rx callback */
 struct xbee_ctx {
        enum xbee_ctx_type type;

diff --git a/rc_proto.c b/rc_proto.c
index 4242477f9c929e3c3bcf51d484aa5ff5bf51907b..495ddccf9543200d1d8cae873023cfea93e9e68d 100644 (file)
--- a/rc_proto.c
+++ b/rc_proto.c
@@ -28,6 +28,102 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <stdint.h>
+#include <event.h>
+#include <linux/joystick.h>
+
+#include <cmdline_parse.h>
+#include <cmdline.h>
 
 #include "rc_proto.h"
+#include "xbee_proto.h"
+#include "joystick.h"
+
+#include "main.h"
+
+
+struct power_levels {
+       int ttl;
+       int power_db;
+};
+
+struct power_levels power_levels[MAX_POWER_LEVEL];
+static struct event power_level_event;
+
+static uint8_t power_level_global;
+
+uint64_t rc_send_dstaddr = 0xFFFF; /* broadcast by default */
+
+static int set_power_level(void *frame, unsigned len, void *arg)
+{
+       struct xbee_atresp_hdr *atresp = (struct xbee_atresp_hdr *)frame;
+       int level = (intptr_t)arg;
+       uint8_t db;
+       db = atresp->data[0];
+
+       power_levels[level].power_db = db;
+       power_levels[level].ttl = 2;
+       return 0;
+}
+
+void rc_proto_rx_range(int power_level)
+{
+       xbeeapp_send_atcmd("DB", NULL, 0, 0,
+                          set_power_level, (void *)(intptr_t)power_level);
+
+}
+
+
+static void compute_best_power_cb(int s, short event, void *arg)
+{
+       struct timeval tv;
+       int i;
+       int best_power_level = MAX_POWER_LEVEL - 1;
+
+       for (i = 0; i < MAX_POWER_LEVEL; i++) {
+               if (power_levels[i].ttl > 0)
+                       power_levels[i].ttl--;
+       }
+       for (i = 0; i < MAX_POWER_LEVEL; i++) {
+               if (power_levels[i].ttl == 0)
+                       continue;
+               if (power_levels[i].power_db < RX_DB_THRESHOLD) {
+                       best_power_level = i;
+                       break;
+               }
+       }
+       /*
+       if (power_level_global != best_power_level)
+               printf("changing power level %d => %d\n",
+                      power_level_global, best_power_level);
+       */
+       power_level_global = best_power_level;
+
+       tv.tv_sec = 1;
+       tv.tv_usec = 0;
+       evtimer_add(&power_level_event, &tv);
+}
+
+void rc_proto_send_channels(void)
+{
+       struct rc_proto_channel rc_chan;
+       int i;
+
+       xbeeapp_send_atcmd("PL", &power_level_global,
+                          sizeof(power_level_global), 0, NULL, NULL);
+       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);
+}
+
+void rc_proto_init(void)
+{
+       struct timeval tv;
+
+       evtimer_set(&power_level_event, compute_best_power_cb, NULL);
+       tv.tv_sec = 1;
+       tv.tv_usec = 0;
+       evtimer_add(&power_level_event, &tv);
+}
 

diff --git a/rc_proto.h b/rc_proto.h
index a9a826b28935806fa442f66ac4034bfb9d6cfec2..dd5895119aee914036a768a445fc09f118b924a4 100644 (file)
--- a/rc_proto.h
+++ b/rc_proto.h
@@ -34,8 +34,16 @@
 #define RC_PROTO_TYPE_RANGE   1
 
 
+#define RX_DB_THRESHOLD 0x40
+
 /* TODO: Authenticate packet!! */
 
+extern uint64_t rc_send_dstaddr;
+
+struct rc_proto_hdr {
+       uint8_t type;
+} __attribute__((packed));
+
 struct rc_proto_channel {
        uint8_t type;
        int16_t axis[AXIS_NUMBER];
@@ -47,4 +55,8 @@ struct rc_proto_range {
 } __attribute__((packed));
 
 
+void rc_proto_rx_range(int power_level);
+void rc_proto_send_channels(void);
+void rc_proto_init(void);
+
 #endif