rc_proto.c

   1 /*
   2  * Copyright (c) 2013, Olivier MATZ <zer0@droids-corp.org>
   3  * All rights reserved.
   4  * Redistribution and use in source and binary forms, with or without
   5  * modification, are permitted provided that the following conditions are met:
   6  *
   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.
  15  *
  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.
  26  */
  27
  28 #include <aversive.h>
  29 #include <aversive/queue.h>
  30
  31 #include <stdint.h>
  32
  33 #include <uart.h>
  34
  35 #include <scheduler.h>
  36 #include <clock_time.h>
  37 #include <parse.h>
  38 #include <rdline.h>
  39 #include <timer.h>
  40 #include <xbee.h>
  41
  42 #include "callout.h"
  43 #include "rc_proto.h"
  44 #include "main.h"
  45
  46 /* */
  47 struct rc_proto_power_levels {
  48         uint8_t ttl;
  49         uint16_t power_db;
  50 };
  51 static struct rc_proto_power_levels power_levels[MAX_POWER_LEVEL];
  52
  53 /* update power level when we receive the answer from DB */
  54 static int update_power_level(void *frame, unsigned len, void *arg)
  55 {
  56         struct xbee_atresp_hdr *atresp = (struct xbee_atresp_hdr *)frame;
  57         int level = (intptr_t)arg;
  58         uint8_t db;
  59
  60         /* XXX check if this test is correct */
  61         if (len < sizeof(struct xbee_atresp_hdr) + sizeof(uint8_t)) {
  62                 /* XXX stats */
  63                 return -1;
  64         }
  65
  66         db = atresp->data[0];
  67         power_levels[level].power_db = db;
  68         power_levels[level].ttl = 2;
  69         return 0;
  70 }
  71
  72 /* when we receive a power probe, ask the DB value to the xbee */
  73 void rc_proto_rx_power_probe(int power_level)
  74 {
  75         (void)power_level;
  76         xbeeapp_send_atcmd("DB", NULL, 0, 0, update_power_level, NULL);
  77 }
  78
  79 #if 0
  80 #define N_SERVO 6
  81 #define SERVO_NBITS 10
  82 uint16_t servos[N_SERVO] = { 0x123, 0x234, 0x321, 0x123, 0x234, 0x321 };
  83
  84 int8_t servo2buf(uint8_t *buf, uint8_t len, uint8_t servo_mask,
  85                   uint8_t pow, uint8_t seq)
  86 {
  87         uint8_t i = 0, num;
  88         uint8_t remain_bits;
  89         uint8_t servo_bits = 0;
  90         uint16_t servo_val;
  91         uint8_t tmp;
  92
  93         if (len < 2)
  94                 return -1;
  95
  96         memset(buf, 0, len);
  97         buf[i++] = servo_mask;
  98         buf[i++] = ((seq & 0x1f) << 3) | (pow & 0x7);
  99         remain_bits = 8;
 100
 101         for (num = 0; num < N_SERVO; num++) {
 102                 if (!(servo_mask & (1 << num)))
 103                         continue;
 104                 servo_val = servos[num];
 105                 servo_bits = SERVO_NBITS;
 106
 107                 tmp = (servo_val >> (servo_bits - remain_bits));
 108                 tmp &= ((1 << remain_bits) - 1);
 109                 if (i >= len)
 110                         return -1;
 111                 buf[i++] |= tmp;
 112
 113                 servo_bits = 10 - remain_bits;
 114                 tmp = servo_val & ((1 << servo_bits) - 1);
 115                 tmp <<= (8 - servo_bits);
 116                 if (i >= len)
 117                         return -1;
 118                 buf[i] = tmp;
 119
 120                 if (servo_bits == 8) {
 121                         i++;
 122                         remain_bits = 8;
 123                 }
 124                 else
 125                         remain_bits = 8 - servo_bits;
 126         }
 127
 128         if (remain_bits != 8)
 129                 i++;
 130
 131         return i;
 132 }
 133
 134 int8_t buf2servo(uint8_t *buf, uint8_t len)
 135 {
 136         uint8_t mask, count = 0;
 137         uint8_t num = 0;
 138         uint8_t pow, seq;
 139         uint16_t val;
 140
 141         if (len < 2)
 142                 return -1;
 143
 144         mask = buf[0];
 145         if (mask > 0x3f)
 146                 return -1;
 147         pow = buf[1] & 0x07;
 148         seq = buf[1] >> 5;
 149
 150         for (num = 0; num < N_SERVO; num++) {
 151                 if ((1<<num) & mask)
 152                         count++;
 153         }
 154         switch (count) {
 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;
 161                 default: return -1;
 162         }
 163
 164         for (num = 0; ((1<<num) & mask) == 0; num++) {
 165                 if (num >= N_SERVO)
 166                         return 0;
 167         }
 168
 169         val = buf[2];
 170         val <<= 2;
 171         val |= (buf[3] >> 6);
 172
 173         for (num++; ((1<<num) & mask) == 0; num++) {
 174                 if (num >= N_SERVO)
 175                         return 0;
 176         }
 177
 178         val = buf[3] & 0x3f;
 179         val <<= 4;
 180         val |= (buf[4] >> 4);
 181
 182         for (num++; ((1<<num) & mask) == 0; num++) {
 183                 if (num >= N_SERVO)
 184                         return 0;
 185         }
 186
 187         val = buf[4] & 0xf;
 188         val <<= 6;
 189         val |= (buf[5] >> 2);
 190
 191         for (num++; ((1<<num) & mask) == 0; num++) {
 192                 if (num >= N_SERVO)
 193                         return 0;
 194         }
 195
 196         val = buf[5] & 0x3;
 197         val <<= 8;
 198         val |= (buf[6]);
 199
 200         for (num++; ((1<<num) & mask) == 0; num++) {
 201                 if (num >= N_SERVO)
 202                         return 0;
 203         }
 204
 205         val = buf[7];
 206         val <<= 2;
 207         val |= (buf[8] >> 6);
 208
 209         for (num++; ((1<<num) & mask) == 0; num++) {
 210                 if (num >= N_SERVO)
 211                         return 0;
 212         }
 213
 214         val = buf[8];
 215         val <<= 4;
 216         val |= (buf[9] >> 4);
 217
 218         return 0;
 219 }
 220 #endif