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 <string.h>
  29
  30 #include <aversive.h>
  31 #include <aversive/queue.h>
  32 #include <aversive/endian.h>
  33
  34 #include <stdint.h>
  35
  36 #include <uart.h>
  37
  38 #include <parse.h>
  39 #include <rdline.h>
  40 #include <timer.h>
  41 #include <xbee.h>
  42
  43 #include "callout.h"
  44 #include "rc_proto.h"
  45 #include "xbee_user.h"
  46 #include "spi_servo.h"
  47 #include "main.h"
  48
  49 #define N_SERVO 6
  50 #define SERVO_NBITS 10
  51
  52 #define RX_DB_THRESHOLD 65 /* mean -65 dB */
  53
  54 /* default values */
  55 struct rc_proto_timers rc_proto_timers = {
  56         .send_servo_min_ms = 50,
  57         .send_servo_max_ms = 300,
  58         .send_power_probe_ms = 500,
  59         .autobypass_ms = 500,
  60 };
  61
  62 /* rc_proto statistics, accessed with sched_prio=XBEE_PRIO */
  63 struct rc_proto_stats_data {
  64         uint32_t hello_rx;
  65         uint32_t hello_tx;
  66         uint32_t echo_req_rx;
  67         uint32_t echo_req_tx;
  68         uint32_t echo_ans_rx;
  69         uint32_t echo_ans_tx;
  70         uint32_t power_probe_rx;
  71         uint32_t power_probe_tx;
  72         uint32_t ack_rx;
  73         uint32_t ack_tx;
  74         uint32_t servo_rx;
  75         uint32_t servo_tx;
  76         uint32_t stats_rx;
  77         uint32_t stats_tx;
  78         uint32_t echo_ans_latency_sum;
  79 };
  80 static struct rc_proto_stats_data stats; /* local stats */
  81 static struct rc_proto_stats_data peer_stats; /* peer stats */
  82
  83 /* store last received power probes */
  84 struct rc_proto_power_levels {
  85         uint8_t ttl;
  86         uint16_t power_db;
  87 };
  88 static struct rc_proto_power_levels power_levels[MAX_POWER_LEVEL];
  89
  90 /* address of the peer */
  91 static uint64_t rc_proto_dstaddr = 0xFFFF; /* broadcast by default */
  92
  93 /* state sent to the xbee peer (used on RC) */
  94 struct servo_tx {
  95         uint16_t servos[N_SERVO];
  96         uint8_t bypass; /* ask the wing to bypass servos = use legacy controller */
  97         uint8_t seq;   /* from 0 to 15, 4 bits */
  98         uint16_t time; /* time of last xmit */
  99 };
 100 static struct servo_tx servo_tx;
 101
 102 /* state received from the xbee peer (used on WING) */
 103 struct servo_rx {
 104         uint16_t servos[N_SERVO];
 105         uint16_t time; /* time of last xmit */
 106 };
 107 static struct servo_rx servo_rx;
 108
 109 /* the received seq value (acknowledged by the wing, received on the rc) */
 110 static uint8_t ack;
 111
 112 /* define tx mode (disabled, send from spi, bypass), rx mode (auto-bypass),
 113    ... */
 114 static uint8_t rc_proto_flags;
 115
 116 /* callout managing rc_proto (ex: sending of servos periodically) */
 117 static struct callout rc_proto_timer;
 118
 119 /* a negative value (-1 or -4) means we don't know the best level, but it stores
 120  * the previous PL value (0 or 4) so we can alternate. */
 121 int8_t power_level_global = -1;
 122
 123 /* update power level when we receive the answer from DB. The request is sent by
 124  * rc_proto_rx_power_probe(). */
 125 static int8_t update_power_level(int8_t retcode, void *frame, unsigned len,
 126         void *arg)
 127 {
 128         struct xbee_atresp_hdr *atresp = (struct xbee_atresp_hdr *)frame;
 129         int level = (intptr_t)arg;
 130         uint8_t db;
 131
 132         /* nothing more to do, error is already logged by xbee_user */
 133         if (retcode < 0)
 134                 return retcode;
 135
 136         if (len < sizeof(struct xbee_atresp_hdr) + sizeof(uint8_t))
 137                 return -1;
 138
 139         db = atresp->data[0];
 140         power_levels[level].power_db = db;
 141         power_levels[level].ttl = 10; /* valid during 10 seconds */
 142         return 0;
 143 }
 144
 145 /* when we receive a power probe, ask the DB value to the xbee */
 146 static void rc_proto_rx_power_probe(int power_level)
 147 {
 148         xbeeapp_send_atcmd("DB", NULL, 0, update_power_level,
 149                 (void *)(intptr_t)power_level);
 150 }
 151
 152 /* called every second to decide which power should be used */
 153 static void compute_best_power(void)
 154 {
 155         int8_t best_power_level = -1;
 156         int8_t i;
 157
 158         /* decrement TTL */
 159         for (i = 0; i < MAX_POWER_LEVEL; i++) {
 160                 if (power_levels[i].ttl > 0)
 161                         power_levels[i].ttl--;
 162         }
 163
 164         for (i = 0; i < MAX_POWER_LEVEL; i++) {
 165                 if (power_levels[i].ttl == 0)
 166                         continue;
 167
 168                 /* if signal is powerful enough, select this as level */
 169                 if (power_levels[i].power_db < RX_DB_THRESHOLD) {
 170                         best_power_level = i;
 171                         break;
 172                 }
 173         }
 174
 175         /* we have no info, don't touch the negative value */
 176         if (best_power_level < 0 && power_level_global < 0)
 177                 return;
 178
 179         if (power_level_global != best_power_level) {
 180                 DEBUG(E_USER_RC_PROTO, "changing power level %d => %d\n",
 181                         power_level_global, best_power_level);
 182         }
 183         power_level_global = best_power_level;
 184 }
 185
 186 /* return the best power level, or -1 if best power level computation is
 187  * disabled. */
 188 static int8_t get_best_power(void)
 189 {
 190         if ((rc_proto_flags & RC_PROTO_FLAGS_COMPUTE_BEST_POW) == 0)
 191                 return -1;
 192
 193         /* special values */
 194         if (power_level_global == -1) {
 195                 power_level_global = -4;
 196                 return 4;
 197         }
 198         else if (power_level_global == -4) {
 199                 power_level_global = -1;
 200                 return 0;
 201         }
 202         else
 203                 return power_level_global;
 204 }
 205
 206 /* send a hello message: no answer expected */
 207 int8_t rc_proto_send_hello(uint64_t addr, void *data, uint8_t data_len,
 208         int8_t power)
 209 {
 210         struct rc_proto_hello hdr;
 211         struct xbee_msg msg;
 212         uint8_t prio;
 213         int8_t ret;
 214
 215         hdr.type = RC_PROTO_HELLO;
 216         hdr.datalen = data_len;
 217
 218         msg.iovlen = 2;
 219         msg.iov[0].buf = &hdr;
 220         msg.iov[0].len = sizeof(hdr);
 221         msg.iov[1].buf = data;
 222         msg.iov[1].len = data_len;
 223
 224         /* set power level */
 225         if (power != -1)
 226                 xbeeapp_send_atcmd("PL", &power, sizeof(power), NULL, NULL);
 227
 228         /* we need to lock callout to increment stats */
 229         prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
 230         stats.hello_tx++;
 231         ret = xbeeapp_send_msg(addr, &msg, NULL, NULL);
 232         callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
 233
 234         return ret;
 235 }
 236
 237 /* send an echo message: expect a reply */
 238 int8_t rc_proto_send_echo_req(uint64_t addr, void *data, uint8_t data_len,
 239         int8_t power)
 240 {
 241         struct rc_proto_echo_req hdr;
 242         struct xbee_msg msg;
 243         uint8_t prio;
 244         int8_t ret;
 245
 246         hdr.type = RC_PROTO_ECHO_REQ;
 247         hdr.power = power;
 248         hdr.timestamp = get_time_ms();
 249         hdr.datalen = data_len;
 250
 251         msg.iovlen = 2;
 252         msg.iov[0].buf = &hdr;
 253         msg.iov[0].len = sizeof(hdr);
 254         msg.iov[1].buf = data;
 255         msg.iov[1].len = data_len;
 256
 257         /* set power level */
 258         if (power != -1)
 259                 xbeeapp_send_atcmd("PL", &power, sizeof(power), NULL, NULL);
 260
 261         /* we need to lock callout to increment stats */
 262         prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
 263         stats.echo_req_tx++;
 264         ret = xbeeapp_send_msg(addr, &msg, NULL, NULL);
 265         callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
 266
 267         return ret;
 268 }
 269
 270 /* send an echo message: expect a reply */
 271 int8_t rc_proto_send_echo_ans(uint64_t addr, void *data, uint8_t data_len,
 272         int8_t power)
 273 {
 274         struct rc_proto_echo_ans hdr;
 275         struct xbee_msg msg;
 276         uint8_t prio;
 277         int8_t ret;
 278
 279         hdr.type = RC_PROTO_ECHO_ANS;
 280         hdr.datalen = data_len;
 281
 282         msg.iovlen = 2;
 283         msg.iov[0].buf = &hdr;
 284         msg.iov[0].len = sizeof(hdr);
 285         msg.iov[1].buf = data;
 286         msg.iov[1].len = data_len;
 287
 288         /* set power level */
 289         if (power != -1)
 290                 xbeeapp_send_atcmd("PL", &power, sizeof(power), NULL, NULL);
 291
 292         /* we need to lock callout to increment stats */
 293         prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
 294         stats.echo_ans_tx++;
 295         ret = xbeeapp_send_msg(addr, &msg, NULL, NULL);
 296         callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
 297
 298         return ret;
 299 }
 300
 301 /* send an echo message: expect a reply */
 302 int8_t rc_proto_send_power_probe(uint64_t addr, uint8_t power)
 303 {
 304         struct rc_proto_power_probe hdr;
 305         struct xbee_msg msg;
 306         uint8_t prio;
 307         int8_t ret;
 308
 309         hdr.type = RC_PROTO_POWER_PROBE;
 310         hdr.power_level = power;
 311
 312         msg.iovlen = 1;
 313         msg.iov[0].buf = &hdr;
 314         msg.iov[0].len = sizeof(hdr);
 315
 316         /* set power level */
 317         xbeeapp_send_atcmd("PL", &power, sizeof(power), NULL, NULL);
 318
 319         /* we need to lock callout to increment stats */
 320         prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
 321         stats.power_probe_tx++;
 322         ret = xbeeapp_send_msg(addr, &msg, NULL, NULL);
 323         callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
 324
 325         return ret;
 326 }
 327
 328 /* convert values from servo_tx.servos into a xbee frame */
 329 static int8_t servo2buf(uint8_t buf[RC_PROTO_SERVO_LEN],
 330         uint8_t seq, uint8_t bypass, uint8_t pow, const uint16_t servos[N_SERVO])
 331 {
 332         uint8_t i = 0;
 333
 334         buf[i++] = RC_PROTO_SERVO;
 335         buf[i++] = ((seq & 0xf) << 4) | (bypass << 3) |  (pow & 0x7);
 336
 337         buf[i++] = servos[0] >> 2;
 338         buf[i] = (servos[0] & 0x3) << 6;
 339
 340         buf[i++] |= servos[1] >> 4;
 341         buf[i] = (servos[1] & 0xf) << 4;
 342
 343         buf[i++] |= servos[2] >> 6;
 344         buf[i] = (servos[2] & 0x3f) << 2;
 345
 346         buf[i++] |= servos[3] >> 8;
 347         buf[i++] = servos[3] & 0xff;
 348
 349         buf[i++] = servos[4] >> 2;
 350         buf[i] = (servos[4] & 0x3) << 6;
 351
 352         buf[i++] |= servos[5] >> 4;
 353         buf[i] = (servos[5] & 0xf) << 4;
 354
 355         return 0;
 356 }
 357
 358 /* send servos, called periodically with prio = XBEE_PRIO */
 359 static int8_t rc_proto_send_servos(void)
 360 {
 361         struct rc_proto_servo hdr;
 362         struct xbee_msg msg;
 363         uint8_t i, updated = 0;
 364         uint16_t ms, diff, servo_val;
 365         uint8_t frame[RC_PROTO_SERVO_LEN];
 366         int8_t ret;
 367         uint8_t power;
 368
 369         /* servo send disabled */
 370         if ((rc_proto_flags & RC_PROTO_FLAGS_TX_MASK) == RC_PROTO_FLAGS_TX_OFF)
 371                 return 0;
 372
 373         /* if we transmitted servos values recently, nothing to do */
 374         ms = get_time_ms();
 375         diff = ms - servo_tx.time;
 376         if (diff < rc_proto_timers.send_servo_min_ms)
 377                 return 0;
 378
 379         /* prepare values to send */
 380         if ((rc_proto_flags & RC_PROTO_FLAGS_TX_MASK) ==
 381                 RC_PROTO_FLAGS_TX_COPY_SPI) {
 382
 383                 /* set bypass to 0 */
 384                 if (servo_tx.bypass == 1) {
 385                         servo_tx.bypass = 0;
 386                         updated = 1;
 387                 }
 388
 389                 /* copy values from spi */
 390                 for (i = 0; i < N_SERVO; i++) {
 391                         servo_val = spi_servo_get(i);
 392                         if (servo_val != servo_tx.servos[i]) {
 393                                 servo_tx.servos[i] = servo_val;
 394                                 updated = 1;
 395                         }
 396                 }
 397         }
 398         else {
 399                 /* set bypass to 1 */
 400                 if (servo_tx.bypass == 0) {
 401                         servo_tx.bypass = 1;
 402                         updated = 1;
 403                 }
 404         }
 405
 406         /* if no value changed and last message is acknowledged, don't transmit
 407          * if we already transmitted quite recently */
 408         if (updated == 0 && ack == servo_tx.seq &&
 409                 diff < rc_proto_timers.send_servo_max_ms)
 410                 return 0;
 411
 412         /* ok, we need to transmit */
 413
 414         /* get the new seq value */
 415         if (updated == 1) {
 416                 servo_tx.seq++;
 417                 servo_tx.seq &= 0x1f;
 418                 if (servo_tx.seq == ack)
 419                         servo_tx.seq = (ack - 1) & 0x1f;
 420         }
 421         /* reset the "updated" flag and save time */
 422         servo_tx.time = ms;
 423
 424         /* set power level */
 425         power = get_best_power();
 426         xbeeapp_send_atcmd("PL", &power, sizeof(power), NULL, NULL);
 427
 428         /* create frame and send it */
 429         servo2buf(frame, servo_tx.seq, servo_tx.bypass, power, servo_tx.servos);
 430         hdr.type = RC_PROTO_SERVO;
 431
 432         msg.iovlen = 2;
 433         msg.iov[0].buf = &hdr;
 434         msg.iov[0].len = sizeof(hdr);
 435         msg.iov[1].buf = frame;
 436         msg.iov[1].len = RC_PROTO_SERVO_LEN;
 437
 438         stats.servo_tx++;
 439         ret = xbeeapp_send_msg(rc_proto_dstaddr, &msg, NULL, NULL);
 440         stats.servo_tx++;
 441
 442         return ret;
 443 }
 444
 445 /* send a ack message: no answer expected */
 446 int8_t rc_proto_send_ack(uint64_t addr, uint8_t seq, int8_t power)
 447 {
 448         struct rc_proto_ack hdr;
 449         struct xbee_msg msg;
 450         uint8_t prio;
 451         int8_t ret;
 452
 453         hdr.type = RC_PROTO_ACK;
 454         hdr.seq = seq;
 455
 456         msg.iovlen = 1;
 457         msg.iov[0].buf = &hdr;
 458         msg.iov[0].len = sizeof(hdr);
 459
 460         /* set power level */
 461         if (power != -1)
 462                 xbeeapp_send_atcmd("PL", &power, sizeof(power), NULL, NULL);
 463
 464         /* we need to lock callout to increment stats */
 465         prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
 466         stats.ack_tx++;
 467         ret = xbeeapp_send_msg(addr, &msg, NULL, NULL);
 468         callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
 469
 470         return ret;
 471 }
 472
 473 /* send a hello message: no answer expected */
 474 int8_t rc_proto_send_stats(uint64_t addr, int8_t power)
 475 {
 476         struct rc_proto_stats hdr;
 477         struct xbee_msg msg;
 478         uint8_t prio;
 479         int8_t ret;
 480
 481         hdr.type = RC_PROTO_STATS;
 482
 483         msg.iovlen = 2;
 484         msg.iov[0].buf = &hdr;
 485         msg.iov[0].len = sizeof(hdr);
 486         msg.iov[1].buf = &stats;
 487         msg.iov[1].len = sizeof(stats);
 488
 489         /* set power level */
 490         if (power != -1)
 491                 xbeeapp_send_atcmd("PL", &power, sizeof(power), NULL, NULL);
 492
 493         /* we need to lock callout to increment stats */
 494         prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
 495         stats.stats_tx++;
 496         ret = xbeeapp_send_msg(addr, &msg, NULL, NULL);
 497         callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
 498
 499         return ret;
 500 }
 501
 502 void rc_proto_set_mode(uint8_t flags)
 503 {
 504         rc_proto_flags = flags;
 505 }
 506
 507 uint8_t rc_proto_get_mode(void)
 508 {
 509         return rc_proto_flags;
 510 }
 511
 512 /* convert a receved servo frame into servo values */
 513 static int8_t buf2servo(uint16_t servos[N_SERVO], const uint8_t *buf)
 514 {
 515         uint16_t val;
 516
 517         val = buf[1];
 518         val <<= 2;
 519         val |= (buf[2] >> 6);
 520         servos[0] = val;
 521
 522         val = buf[2] & 0x3f;
 523         val <<= 4;
 524         val |= (buf[3] >> 4);
 525         servos[1] = val;
 526
 527         val = buf[3] & 0xf;
 528         val <<= 6;
 529         val |= (buf[4] >> 2);
 530         servos[2] = val;
 531
 532         val = buf[4] & 0x3;
 533         val <<= 8;
 534         val |= (buf[5]);
 535         servos[3] = val;
 536
 537         val = buf[6];
 538         val <<= 2;
 539         val |= (buf[7] >> 6);
 540         servos[4] = val;
 541
 542         val = buf[7];
 543         val <<= 4;
 544         val |= (buf[8] >> 4);
 545         servos[5] = val;
 546
 547         return 0;
 548 }
 549
 550 /* process a received servo frame */
 551 static int8_t rc_proto_rx_servo(struct rc_proto_servo *rcs)
 552 {
 553         uint8_t bypass;
 554         uint8_t i, seq, pow;
 555
 556         bypass = !!(rcs->data[0] & 0x08);
 557         pow = rcs->data[0] & 0x07;
 558
 559         /* convert it in a table of servo values */
 560         if (bypass == 0 && buf2servo(servo_rx.servos, rcs->data) < 0)
 561                 return -1;
 562
 563         /* save time */
 564         servo_rx.time = get_time_ms();
 565
 566         /* acknowledge received frame */
 567         seq = rcs->data[0] >> 4;
 568         rc_proto_send_ack(rc_proto_dstaddr, seq, pow);
 569
 570         /* copy values to spi */
 571         if (rc_proto_flags & RC_PROTO_FLAGS_RX_COPY_SPI) {
 572                 spi_servo_set_bypass(bypass);
 573
 574                 if (bypass == 0) {
 575                         for (i = 0; i < N_SERVO; i++)
 576                                 spi_servo_set(i, servo_rx.servos[i]);
 577                 }
 578         }
 579         return 0;
 580 }
 581
 582 /* receive a rc_proto message */
 583 int rc_proto_rx(struct xbee_recv_hdr *recvframe, unsigned len)
 584 {
 585         unsigned int datalen;
 586         struct rc_proto_hdr *rch = (struct rc_proto_hdr *) &recvframe->data;
 587
 588         if (len <  sizeof(*recvframe))
 589                 return -1;
 590
 591         datalen = len - sizeof(*recvframe);
 592         if (datalen < sizeof(struct rc_proto_hdr))
 593                 return -1;
 594
 595         /* other command types */
 596         switch (rch->type) {
 597                 case RC_PROTO_HELLO: {
 598                         struct rc_proto_hello *rch =
 599                                 (struct rc_proto_hello *) recvframe->data;
 600
 601                         NOTICE(E_USER_XBEE, "recv hello len=%d", rch->datalen);
 602                         stats.hello_rx++;
 603                         return 0;
 604                 }
 605
 606                 case RC_PROTO_ECHO_REQ: {
 607                         struct rc_proto_echo_req *rce =
 608                                 (struct rc_proto_echo_req *) recvframe->data;
 609                         int8_t power = rce->power;
 610
 611                         NOTICE(E_USER_XBEE, "recv echo len=%d", rce->datalen);
 612                         stats.echo_req_rx++;
 613
 614                         if (rc_proto_send_echo_ans(ntohll(recvframe->srcaddr),
 615                                         rce->data, rce->datalen, power) < 0)
 616                                 return -1;
 617
 618                         return 0;
 619                 }
 620
 621                 case RC_PROTO_ECHO_ANS: {
 622                         struct rc_proto_echo_ans *rce =
 623                                 (struct rc_proto_echo_ans *) recvframe->data;
 624                         uint16_t diff;
 625
 626                         NOTICE(E_USER_XBEE, "recv echo_ans len=%d", rce->datalen);
 627                         stats.echo_ans_rx++;
 628                         diff = get_time_ms() - rce->timestamp;
 629                         stats.echo_ans_latency_sum += diff;
 630                         return 0;
 631                 }
 632
 633                 /* received by the radio controller every ~500ms */
 634                 case RC_PROTO_POWER_PROBE: {
 635                         struct rc_proto_power_probe *rcpb =
 636                                 (struct rc_proto_power_probe *) recvframe->data;
 637
 638                         NOTICE(E_USER_XBEE, "recv power_probe");
 639
 640                         if (datalen != sizeof(*rcpb))
 641                                 return -1;
 642
 643                         if (rcpb->power_level >= MAX_POWER_LEVEL)
 644                                 return -1;
 645
 646                         stats.power_probe_rx++;
 647                         /* ask the DB value to the xbee module */
 648                         rc_proto_rx_power_probe(rcpb->power_level);
 649
 650                         return 0;
 651                 }
 652
 653                 /* received by the radio controller */
 654                 case RC_PROTO_ACK: {
 655                         struct rc_proto_ack *rca =
 656                                 (struct rc_proto_ack *) recvframe->data;
 657
 658                         NOTICE(E_USER_XBEE, "recv ack, ack=%d", rca->seq);
 659                         stats.ack_rx++;
 660                         return 0;
 661                 }
 662
 663                 /* received by the wing */
 664                 case RC_PROTO_SERVO: {
 665                         struct rc_proto_servo *rcs =
 666                                 (struct rc_proto_servo *) recvframe->data;
 667
 668                         NOTICE(E_USER_XBEE, "recv servo");
 669
 670                         if (datalen != RC_PROTO_SERVO_LEN)
 671                                 return -1;
 672
 673                         stats.servo_rx++;
 674                         return rc_proto_rx_servo(rcs);
 675                 }
 676
 677                 /* received by the radio controller */
 678                 case RC_PROTO_STATS: {
 679                         struct rc_proto_stats *rcs =
 680                                 (struct rc_proto_stats *) recvframe->data;
 681
 682                         NOTICE(E_USER_XBEE, "recv stats");
 683
 684                         if (datalen != sizeof(*rcs) + sizeof(peer_stats))
 685                                 return -1;
 686
 687                         stats.stats_rx++;
 688                         memcpy(&peer_stats, rcs->stats, sizeof(peer_stats));
 689                         return 0;
 690                 }
 691
 692                 default:
 693                         return -1;
 694         }
 695
 696         /* not reached */
 697         return 0;
 698 }
 699
 700 /* called by the scheduler, manage rc_proto periodical tasks */
 701 static void rc_proto_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
 702 {
 703         (void)arg;
 704         static uint16_t prev_stats_send;
 705         static uint16_t prev_compute_pow;
 706         static uint16_t prev_power_probe;
 707         static uint8_t pow_probe;
 708         uint16_t t, diff;
 709
 710         t = get_time_ms();
 711
 712         /* send servo values if flags are enabled. The function will decide
 713          * by itself if it's time to send or not */
 714         rc_proto_send_servos();
 715
 716         /* send power probe periodically */
 717         if (rc_proto_flags & RC_PROTO_FLAGS_TX_POW_PROBE) {
 718                 diff = t - prev_power_probe;
 719                 if (diff > rc_proto_timers.send_power_probe_ms) {
 720                         pow_probe++;
 721                         if (pow_probe > 4)
 722                                 pow_probe = 0;
 723                         rc_proto_send_power_probe(rc_proto_dstaddr, pow_probe);
 724                         prev_power_probe = t;
 725                 }
 726         }
 727
 728         /* on wing, auto bypass servos if no commands since some time */
 729         if (rc_proto_flags & RC_PROTO_FLAGS_RX_AUTOBYPASS) {
 730                 diff = t - servo_rx.time;
 731                 if (diff > rc_proto_timers.autobypass_ms)
 732                         spi_servo_set_bypass(1);
 733         }
 734
 735         /* send stats to peer every second */
 736         if (rc_proto_flags & RC_PROTO_FLAGS_COMPUTE_BEST_POW) {
 737                 diff = t - prev_compute_pow;
 738                 if (diff >= 1000) {
 739                         compute_best_power();
 740                         prev_compute_pow = t;
 741                 }
 742         }
 743
 744         /* send stats to peer every second */
 745         if (rc_proto_flags & RC_PROTO_FLAGS_TX_STATS) {
 746                 diff = t - prev_stats_send;
 747                 if (diff >= 1000) {
 748                         rc_proto_send_stats(rc_proto_dstaddr, get_best_power());
 749                         prev_stats_send = t;
 750                 }
 751         }
 752
 753         callout_schedule(cm, tim, 0);
 754 }
 755
 756 void rc_proto_dump_stats(void)
 757 {
 758         printf_P(PSTR("rc_proto stats LOCAL\r\n"));
 759         printf_P(PSTR("  hello_tx: %"PRIu32"\r\n"), stats.hello_tx);
 760         printf_P(PSTR("  hello_rx: %"PRIu32"\r\n"), stats.hello_rx);
 761         printf_P(PSTR("  echo_req_rx: %"PRIu32"\r\n"), stats.echo_req_rx);
 762         printf_P(PSTR("  echo_req_tx: %"PRIu32"\r\n"), stats.echo_req_tx);
 763         printf_P(PSTR("  echo_ans_rx: %"PRIu32"\r\n"), stats.echo_ans_rx);
 764         printf_P(PSTR("  echo_ans_tx: %"PRIu32"\r\n"), stats.echo_ans_tx);
 765         printf_P(PSTR("  power_probe_rx: %"PRIu32"\r\n"), stats.power_probe_rx);
 766         printf_P(PSTR("  power_probe_tx: %"PRIu32"\r\n"), stats.power_probe_tx);
 767         printf_P(PSTR("  ack_rx: %"PRIu32"\r\n"), stats.ack_rx);
 768         printf_P(PSTR("  ack_tx: %"PRIu32"\r\n"), stats.ack_tx);
 769         printf_P(PSTR("  servo_rx: %"PRIu32"\r\n"), stats.servo_rx);
 770         printf_P(PSTR("  servo_tx: %"PRIu32"\r\n"), stats.servo_tx);
 771         printf_P(PSTR("  stats_rx: %"PRIu32"\r\n"), stats.stats_rx);
 772         printf_P(PSTR("  stats_tx: %"PRIu32"\r\n"), stats.stats_tx);
 773         if (stats.echo_ans_rx != 0) {
 774                 printf_P(PSTR("  echo_ans_latency_ms: %"PRIu32"\r\n"),
 775                         stats.echo_ans_latency_sum / stats.echo_ans_rx);
 776         }
 777
 778         printf_P(PSTR("rc_proto stats PEER\r\n"));
 779         printf_P(PSTR("  hello_tx: %"PRIu32"\r\n"), peer_stats.hello_tx);
 780         printf_P(PSTR("  hello_rx: %"PRIu32"\r\n"), peer_stats.hello_rx);
 781         printf_P(PSTR("  echo_req_rx: %"PRIu32"\r\n"), peer_stats.echo_req_rx);
 782         printf_P(PSTR("  echo_req_tx: %"PRIu32"\r\n"), peer_stats.echo_req_tx);
 783         printf_P(PSTR("  echo_ans_rx: %"PRIu32"\r\n"), peer_stats.echo_ans_rx);
 784         printf_P(PSTR("  echo_ans_tx: %"PRIu32"\r\n"), peer_stats.echo_ans_tx);
 785         printf_P(PSTR("  power_probe_rx: %"PRIu32"\r\n"), peer_stats.power_probe_rx);
 786         printf_P(PSTR("  power_probe_tx: %"PRIu32"\r\n"), peer_stats.power_probe_tx);
 787         printf_P(PSTR("  ack_rx: %"PRIu32"\r\n"), peer_stats.ack_rx);
 788         printf_P(PSTR("  ack_tx: %"PRIu32"\r\n"), peer_stats.ack_tx);
 789         printf_P(PSTR("  servo_rx: %"PRIu32"\r\n"), peer_stats.servo_rx);
 790         printf_P(PSTR("  servo_tx: %"PRIu32"\r\n"), peer_stats.servo_tx);
 791         printf_P(PSTR("  stats_rx: %"PRIu32"\r\n"), peer_stats.stats_rx);
 792         printf_P(PSTR("  stats_tx: %"PRIu32"\r\n"), peer_stats.stats_tx);
 793         if (stats.echo_ans_rx != 0) {
 794                 printf_P(PSTR("  echo_ans_latency_ms: %"PRIu32"\r\n"),
 795                         peer_stats.echo_ans_latency_sum / peer_stats.echo_ans_rx);
 796         }
 797 }
 798
 799 void rc_proto_reset_stats(void)
 800 {
 801         uint8_t prio;
 802
 803         prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
 804         memset(&stats, 0, sizeof(stats));
 805         callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
 806 }
 807
 808 void rc_proto_dump_servos(void)
 809 {
 810         uint8_t i;
 811
 812         printf_P(PSTR("servo rx\r\n"));
 813         for (i = 0; i < N_SERVO; i++) {
 814                 printf_P(PSTR("  servo[%d] = %d\r\n"), i, servo_rx.servos[i]);
 815         }
 816         printf_P(PSTR("servo tx\r\n"));
 817         printf_P(PSTR("  bypass=%d\r\n"), servo_tx.bypass);
 818         printf_P(PSTR("  seq=%d\r\n"), servo_tx.seq);
 819         printf_P(PSTR("  time=%d\r\n"), servo_tx.time);
 820         for (i = 0; i < N_SERVO; i++) {
 821                 printf_P(PSTR("  servo[%d] = %d\r\n"), i, servo_tx.servos[i]);
 822         }
 823 }
 824
 825 void rc_proto_set_dstaddr(uint64_t addr)
 826 {
 827         uint8_t flags;
 828
 829         IRQ_LOCK(flags);
 830         rc_proto_dstaddr = addr;
 831         IRQ_UNLOCK(flags);
 832 }
 833
 834 uint64_t rc_proto_get_dstaddr(void)
 835 {
 836         uint64_t addr;
 837         uint8_t flags;
 838
 839         IRQ_LOCK(flags);
 840         addr = rc_proto_dstaddr;
 841         IRQ_UNLOCK(flags);
 842         return addr;
 843 }
 844
 845 void rc_proto_init(void)
 846 {
 847         callout_init(&rc_proto_timer, rc_proto_cb, NULL, XBEE_PRIO);
 848         callout_schedule(&xbeeboard.intr_cm, &rc_proto_timer, 0);
 849 }