use xbee module from aversive

[protos/xbee-avr.git] / rc_proto.c

diff --git a/rc_proto.c b/rc_proto.c
index 9e057e3..fe5c928 100644 (file)
--- 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