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.
29 #include <aversive/queue.h>
35 #include <scheduler.h>
36 #include <clock_time.h>
47 struct rc_proto_power_levels {
51 static struct rc_proto_power_levels power_levels[MAX_POWER_LEVEL];
53 /* update power level when we receive the answer from DB */
54 static int update_power_level(void *frame, unsigned len, void *arg)
56 struct xbee_atresp_hdr *atresp = (struct xbee_atresp_hdr *)frame;
57 int level = (intptr_t)arg;
60 /* XXX check if this test is correct */
61 if (len < sizeof(struct xbee_atresp_hdr) + sizeof(uint8_t)) {
67 power_levels[level].power_db = db;
68 power_levels[level].ttl = 2;
72 /* when we receive a power probe, ask the DB value to the xbee */
73 void rc_proto_rx_power_probe(int power_level)
76 xbeeapp_send_atcmd("DB", NULL, 0, 0, update_power_level, NULL);
81 #define SERVO_NBITS 10
82 uint16_t servos[N_SERVO] = { 0x123, 0x234, 0x321, 0x123, 0x234, 0x321 };
84 int8_t servo2buf(uint8_t *buf, uint8_t len, uint8_t servo_mask,
85 uint8_t pow, uint8_t seq)
89 uint8_t servo_bits = 0;
97 buf[i++] = servo_mask;
98 buf[i++] = ((seq & 0x1f) << 3) | (pow & 0x7);
101 for (num = 0; num < N_SERVO; num++) {
102 if (!(servo_mask & (1 << num)))
104 servo_val = servos[num];
105 servo_bits = SERVO_NBITS;
107 tmp = (servo_val >> (servo_bits - remain_bits));
108 tmp &= ((1 << remain_bits) - 1);
113 servo_bits = 10 - remain_bits;
114 tmp = servo_val & ((1 << servo_bits) - 1);
115 tmp <<= (8 - servo_bits);
120 if (servo_bits == 8) {
125 remain_bits = 8 - servo_bits;
128 if (remain_bits != 8)
134 int8_t buf2servo(uint8_t *buf, uint8_t len)
136 uint8_t mask, count = 0;
150 for (num = 0; num < N_SERVO; num++) {
155 case 1: if (len != 4) return -1; break;
156 case 2: if (len != 5) return -1; break;
157 case 3: if (len != 6) return -1; break;
158 case 4: if (len != 7) return -1; break;
159 case 5: if (len != 9) return -1; break;
160 case 6: if (len != 10) return -1; break;
164 for (num = 0; ((1<<num) & mask) == 0; num++) {
171 val |= (buf[3] >> 6);
173 for (num++; ((1<<num) & mask) == 0; num++) {
180 val |= (buf[4] >> 4);
182 for (num++; ((1<<num) & mask) == 0; num++) {
189 val |= (buf[5] >> 2);
191 for (num++; ((1<<num) & mask) == 0; num++) {
200 for (num++; ((1<<num) & mask) == 0; num++) {
207 val |= (buf[8] >> 6);
209 for (num++; ((1<<num) & mask) == 0; num++) {
216 val |= (buf[9] >> 4);