rc_proto: choose best power level for rc transmit

[protos/xbee.git] / rc_proto.c

diff --git a/rc_proto.c b/rc_proto.c
index 4242477..495ddcc 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);
+}