X-Git-Url: http://git.droids-corp.org/?p=protos%2Fxbee-avr.git;a=blobdiff_plain;f=rc_proto.c;h=56e8ad60333b398e8f0c3637082ab0e510450d2d;hp=ee2888bb3f43de05dc4823a71bff2cf58d7a697f;hb=4d9670d6245cc381313a3ff41dd80f6596a266c8;hpb=d6014994e06ebe952d51bef2e36a795f624c4b4d diff --git a/rc_proto.c b/rc_proto.c index ee2888b..56e8ad6 100644 --- a/rc_proto.c +++ b/rc_proto.c @@ -1,3 +1,32 @@ +/* + * Copyright (c) 2013, Olivier MATZ + * 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 + #include #include @@ -5,44 +34,207 @@ #include -#include -#include #include #include #include +#include -#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 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; - db = atresp->data[0]; + /* 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, 0, update_power_level, NULL); +} + +/* send a hello message */ +int8_t rc_proto_send_hello(uint64_t addr, void *data, uint8_t data_len) { - xbeeapp_send_atcmd("DB", NULL, 0, 0, - set_power_level, (void *)(intptr_t)power_level); + 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, 1); } +#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) +{ + 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<= N_SERVO) + return 0; + } + + val = buf[2]; + val <<= 2; + val |= (buf[3] >> 6); + + for (num++; ((1<= N_SERVO) + return 0; + } + + val = buf[3] & 0x3f; + val <<= 4; + val |= (buf[4] >> 4); + + for (num++; ((1<= N_SERVO) + return 0; + } + + val = buf[4] & 0xf; + val <<= 6; + val |= (buf[5] >> 2); + + for (num++; ((1<= N_SERVO) + return 0; + } + + val = buf[5] & 0x3; + val <<= 8; + val |= (buf[6]); + + for (num++; ((1<= N_SERVO) + return 0; + } + + val = buf[7]; + val <<= 2; + val |= (buf[8] >> 6); + + for (num++; ((1<= N_SERVO) + return 0; + } + + val = buf[8]; + val <<= 4; + val |= (buf[9] >> 4); + + return 0; +} +#endif