+/*
+ * Copyright (c) 2013, Olivier MATZ <zer0@droids-corp.org>
+ * All rights reserved.
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the University of California, Berkeley nor the
+ * names of its contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <string.h>
+
#include <aversive.h>
#include <aversive/queue.h>
#include <uart.h>
-#include <scheduler.h>
-#include <clock_time.h>
#include <parse.h>
#include <rdline.h>
#include <timer.h>
+#include <xbee.h>
-#include "xbee_proto.h"
#include "callout.h"
#include "rc_proto.h"
+#include "xbee_user.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 int8_t update_power_level(int8_t retcode, 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];
+ /* nothing more to do, error is already logged by xbee_user */
+ if (retcode < 0)
+ return retcode;
+
+ /* XXX check if this test is correct */
+ if (len < sizeof(struct xbee_atresp_hdr) + sizeof(uint8_t)) {
+ /* XXX stats */
+ return -1;
+ }
+
+ 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)
+/* 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, update_power_level, NULL);
+}
+
+/* send a hello message */
+int8_t rc_proto_send_hello(uint64_t addr, void *data, uint8_t data_len)
+{
+ struct rc_proto_echo_req hdr;
+ struct xbee_msg msg;
+
+ hdr.type = RC_PROTO_HELLO;
+ hdr.datalen = data_len;
+
+ msg.iovlen = 2;
+ msg.iov[0].buf = &hdr;
+ msg.iov[0].len = sizeof(hdr);
+ msg.iov[1].buf = data;
+ msg.iov[1].len = data_len;
+
+ return xbeeapp_send_msg(addr, &msg, NULL, 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)
+{
+ 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)
{
- xbeeapp_send_atcmd("DB", NULL, 0, 0,
- set_power_level, (void *)(intptr_t)power_level);
+ 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
+
+int rc_proto_rx(struct xbee_recv_hdr *recvframe, unsigned len)
+{
+ unsigned int datalen;
+ struct rc_proto_hdr *rch = (struct rc_proto_hdr *) &recvframe->data;
+
+ if (len < sizeof(*recvframe))
+ return -1;
+
+ datalen = len - sizeof(*recvframe);
+ if (datalen < sizeof(struct rc_proto_hdr))
+ return -1;
+
+ switch (rch->type) {
+#if 0
+ case RC_PROTO_TYPE_CHANNEL: {
+ struct rc_proto_channel *rcc =
+ (struct rc_proto_channel *) recvframe->data;
+ int16_t val;
+ if (datalen != sizeof(struct rc_proto_channel))
+ return -1;
+ val = ntohs(rcc->axis[0]);
+ val >>= 6;
+ val += 512;
+ spi_servo_set(0, val);
+ break;
+ }
+#endif
+ case RC_PROTO_POWER_PROBE: {
+ struct rc_proto_power_probe *rcpb =
+ (struct rc_proto_power_probe *) recvframe->data;
+ if (datalen != sizeof(*rcpb))
+ return -1;
+ if (rcpb->power_level >= MAX_POWER_LEVEL)
+ return -1;
+
+ //rc_proto_rx_range(rcpb->power_level);
+
+ break;
+ }
+
+ case RC_PROTO_HELLO: {
+ struct rc_proto_hello *rch =
+ (struct rc_proto_hello *) recvframe->data;
+
+ NOTICE(E_USER_XBEE, "recv hello len=%d",
+ rch->datalen);
+ /* XXX stats */
+ break;
+ }
+ default:
+ return -1;
+ }
+
+ return 0;
+}