X-Git-Url: http://git.droids-corp.org/?p=protos%2Fxbee.git;a=blobdiff_plain;f=rc_proto.c;fp=rc_proto.c;h=495ddccf9543200d1d8cae873023cfea93e9e68d;hp=4242477f9c929e3c3bcf51d484aa5ff5bf51907b;hb=93e278d345d286301b3d0b4fe209dd04c734d7bf;hpb=9fdf7fa123e328a13ade691f4b724a187ee39a85

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