2 * Copyright (c) 2013, Olivier MATZ <zer0@droids-corp.org>
4 * Redistribution and use in source and binary forms, with or without
5 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the University of California, Berkeley nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
16 * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31 #include <aversive/queue.h>
44 #include "xbee_user.h"
48 struct rc_proto_power_levels {
52 static struct rc_proto_power_levels power_levels[MAX_POWER_LEVEL];
54 /* update power level when we receive the answer from DB */
55 static int8_t update_power_level(int8_t retcode, void *frame, unsigned len,
58 struct xbee_atresp_hdr *atresp = (struct xbee_atresp_hdr *)frame;
59 int level = (intptr_t)arg;
62 /* nothing more to do, error is already logged by xbee_user */
66 /* XXX check if this test is correct */
67 if (len < sizeof(struct xbee_atresp_hdr) + sizeof(uint8_t)) {
73 power_levels[level].power_db = db;
74 power_levels[level].ttl = 2;
78 /* when we receive a power probe, ask the DB value to the xbee */
79 void rc_proto_rx_power_probe(int power_level)
82 xbeeapp_send_atcmd("DB", NULL, 0, update_power_level, NULL);
85 /* send a hello message */
86 int8_t rc_proto_send_hello(uint64_t addr, void *data, uint8_t data_len)
88 struct rc_proto_echo_req hdr;
91 hdr.type = RC_PROTO_HELLO;
92 hdr.datalen = data_len;
95 msg.iov[0].buf = &hdr;
96 msg.iov[0].len = sizeof(hdr);
97 msg.iov[1].buf = data;
98 msg.iov[1].len = data_len;
100 return xbeeapp_send_msg(addr, &msg, NULL, NULL);
106 #define SERVO_NBITS 10
107 uint16_t servos[N_SERVO] = { 0x123, 0x234, 0x321, 0x123, 0x234, 0x321 };
109 int8_t servo2buf(uint8_t *buf, uint8_t len, uint8_t servo_mask,
110 uint8_t pow, uint8_t seq)
114 uint8_t servo_bits = 0;
122 buf[i++] = servo_mask;
123 buf[i++] = ((seq & 0x1f) << 3) | (pow & 0x7);
126 for (num = 0; num < N_SERVO; num++) {
127 if (!(servo_mask & (1 << num)))
129 servo_val = servos[num];
130 servo_bits = SERVO_NBITS;
132 tmp = (servo_val >> (servo_bits - remain_bits));
133 tmp &= ((1 << remain_bits) - 1);
138 servo_bits = 10 - remain_bits;
139 tmp = servo_val & ((1 << servo_bits) - 1);
140 tmp <<= (8 - servo_bits);
145 if (servo_bits == 8) {
150 remain_bits = 8 - servo_bits;
153 if (remain_bits != 8)
159 int8_t buf2servo(uint8_t *buf, uint8_t len)
161 uint8_t mask, count = 0;
175 for (num = 0; num < N_SERVO; num++) {
180 case 1: if (len != 4) return -1; break;
181 case 2: if (len != 5) return -1; break;
182 case 3: if (len != 6) return -1; break;
183 case 4: if (len != 7) return -1; break;
184 case 5: if (len != 9) return -1; break;
185 case 6: if (len != 10) return -1; break;
189 for (num = 0; ((1<<num) & mask) == 0; num++) {
196 val |= (buf[3] >> 6);
198 for (num++; ((1<<num) & mask) == 0; num++) {
205 val |= (buf[4] >> 4);
207 for (num++; ((1<<num) & mask) == 0; num++) {
214 val |= (buf[5] >> 2);
216 for (num++; ((1<<num) & mask) == 0; num++) {
225 for (num++; ((1<<num) & mask) == 0; num++) {
232 val |= (buf[8] >> 6);
234 for (num++; ((1<<num) & mask) == 0; num++) {
241 val |= (buf[9] >> 4);
247 int rc_proto_rx(struct xbee_recv_hdr *recvframe, unsigned len)
249 unsigned int datalen;
250 struct rc_proto_hdr *rch = (struct rc_proto_hdr *) &recvframe->data;
252 if (len < sizeof(*recvframe))
255 datalen = len - sizeof(*recvframe);
256 if (datalen < sizeof(struct rc_proto_hdr))
261 case RC_PROTO_TYPE_CHANNEL: {
262 struct rc_proto_channel *rcc =
263 (struct rc_proto_channel *) recvframe->data;
265 if (datalen != sizeof(struct rc_proto_channel))
267 val = ntohs(rcc->axis[0]);
270 spi_servo_set(0, val);
274 case RC_PROTO_POWER_PROBE: {
275 struct rc_proto_power_probe *rcpb =
276 (struct rc_proto_power_probe *) recvframe->data;
278 if (datalen != sizeof(*rcpb))
281 if (rcpb->power_level >= MAX_POWER_LEVEL)
284 //rc_proto_rx_range(rcpb->power_level);
289 case RC_PROTO_HELLO: {
290 struct rc_proto_hello *rch =
291 (struct rc_proto_hello *) recvframe->data;
293 NOTICE(E_USER_XBEE, "recv hello len=%d",