X-Git-Url: http://git.droids-corp.org/?p=protos%2Fxbee-avr.git;a=blobdiff_plain;f=rc_proto.c;h=fe5c928bcc048ea45e2847e48b85a4d347a3b6c8;hp=9e057e36b6980425876aa07426f3c3b8e4c0142c;hb=d590ebe19bfe60a544717606a0ff2b348cc32229;hpb=4989a2c76e8fa25667663a215709f9366747fcad

diff --git a/rc_proto.c b/rc_proto.c
index 9e057e3..fe5c928 100644
--- a/rc_proto.c
+++ b/rc_proto.c
@@ -37,28 +37,31 @@
 #include <parse.h>
 #include <rdline.h>
 #include <timer.h>
+#include <xbee.h>
 
-#include "xbee_proto.h"
 #include "callout.h"
 #include "rc_proto.h"
 #include "main.h"
 
-struct power_levels {
-	int ttl;
-	int power_db;
+/* */
+struct rc_proto_power_levels {
+	uint8_t ttl;
+	uint16_t power_db;
 };
+static struct rc_proto_power_levels power_levels[MAX_POWER_LEVEL];
 
-struct power_levels power_levels[MAX_POWER_LEVEL];
-
-static int set_power_level(void *frame, unsigned len, void *arg)
+/* update power level when we receive the answer from DB */
+static int update_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;
 
 	/* XXX check if this test is correct */
-	if (len < sizeof(struct xbee_atresp_hdr) + sizeof(uint8_t))
+	if (len < sizeof(struct xbee_atresp_hdr) + sizeof(uint8_t)) {
+		/* XXX stats */
 		return -1;
+	}
 
 	db = atresp->data[0];
 	power_levels[level].power_db = db;
@@ -66,11 +69,152 @@ static int set_power_level(void *frame, unsigned len, void *arg)
 	return 0;
 }
 
-void rc_proto_rx_range(int power_level)
+/* when we receive a power probe, ask the DB value to the xbee */
+void rc_proto_rx_power_probe(int power_level)
+{
+	(void)power_level;
+	xbeeapp_send_atcmd("DB", NULL, 0, 0, update_power_level, NULL);
+}
+
+#if 0
+#define N_SERVO 6
+#define SERVO_NBITS 10
+uint16_t servos[N_SERVO] = { 0x123, 0x234, 0x321, 0x123, 0x234, 0x321 };
+
+int8_t servo2buf(uint8_t *buf, uint8_t len, uint8_t servo_mask,
+		  uint8_t pow, uint8_t seq)
 {
-	xbeeapp_send_atcmd("DB", NULL, 0, 0,
-			   set_power_level, (void *)(intptr_t)power_level);
+	uint8_t i = 0, num;
+	uint8_t remain_bits;
+	uint8_t servo_bits = 0;
+	uint16_t servo_val;
+	uint8_t tmp;
+
+	if (len < 2)
+		return -1;
+
+	memset(buf, 0, len);
+	buf[i++] = servo_mask;
+	buf[i++] = ((seq & 0x1f) << 3) | (pow & 0x7);
+	remain_bits = 8;
+
+	for (num = 0; num < N_SERVO; num++) {
+		if (!(servo_mask & (1 << num)))
+			continue;
+		servo_val = servos[num];
+		servo_bits = SERVO_NBITS;
 
+		tmp = (servo_val >> (servo_bits - remain_bits));
+		tmp &= ((1 << remain_bits) - 1);
+		if (i >= len)
+			return -1;
+		buf[i++] |= tmp;
+
+		servo_bits = 10 - remain_bits;
+		tmp = servo_val & ((1 << servo_bits) - 1);
+		tmp <<= (8 - servo_bits);
+		if (i >= len)
+			return -1;
+		buf[i] = tmp;
+
+		if (servo_bits == 8) {
+			i++;
+			remain_bits = 8;
+		}
+		else
+			remain_bits = 8 - servo_bits;
+	}
+
+	if (remain_bits != 8)
+		i++;
+
+	return i;
 }
 
+int8_t buf2servo(uint8_t *buf, uint8_t len)
+{
+	uint8_t mask, count = 0;
+	uint8_t num = 0;
+	uint8_t pow, seq;
+	uint16_t val;
+
+	if (len < 2)
+		return -1;
 
+	mask = buf[0];
+	if (mask > 0x3f)
+		return -1;
+	pow = buf[1] & 0x07;
+	seq = buf[1] >> 5;
+
+	for (num = 0; num < N_SERVO; num++) {
+		if ((1<<num) & mask)
+			count++;
+	}
+	switch (count) {
+		case 1: if (len != 4) return -1; break;
+		case 2: if (len != 5) return -1; break;
+		case 3: if (len != 6) return -1; break;
+		case 4: if (len != 7) return -1; break;
+		case 5: if (len != 9) return -1; break;
+		case 6: if (len != 10) return -1; break;
+		default: return -1;
+	}
+
+	for (num = 0; ((1<<num) & mask) == 0; num++) {
+		if (num >= N_SERVO)
+			return 0;
+	}
+
+	val = buf[2];
+	val <<= 2;
+	val |= (buf[3] >> 6);
+
+	for (num++; ((1<<num) & mask) == 0; num++) {
+		if (num >= N_SERVO)
+			return 0;
+	}
+
+	val = buf[3] & 0x3f;
+	val <<= 4;
+	val |= (buf[4] >> 4);
+
+	for (num++; ((1<<num) & mask) == 0; num++) {
+		if (num >= N_SERVO)
+			return 0;
+	}
+
+	val = buf[4] & 0xf;
+	val <<= 6;
+	val |= (buf[5] >> 2);
+
+	for (num++; ((1<<num) & mask) == 0; num++) {
+		if (num >= N_SERVO)
+			return 0;
+	}
+
+	val = buf[5] & 0x3;
+	val <<= 8;
+	val |= (buf[6]);
+
+	for (num++; ((1<<num) & mask) == 0; num++) {
+		if (num >= N_SERVO)
+			return 0;
+	}
+
+	val = buf[7];
+	val <<= 2;
+	val |= (buf[8] >> 6);
+
+	for (num++; ((1<<num) & mask) == 0; num++) {
+		if (num >= N_SERVO)
+			return 0;
+	}
+
+	val = buf[8];
+	val <<= 4;
+	val |= (buf[9] >> 4);
+
+	return 0;
+}
+#endif