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 */
 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 static uint8_t get_best_power(void)
 187 {
 188         /* special values */
 189         if (power_level_global == -1) {
 190                 power_level_global = -4;
 191                 return 4;
 192         }
 193         else if (power_level_global == -4) {
 194                 power_level_global = -1;
 195                 return 0;
 196         }
 197         else
 198                 return power_level_global;
 199 }
 200
 201 /* send a hello message: no answer expected */
 202 int8_t rc_proto_send_hello(uint64_t addr, void *data, uint8_t data_len,
 203         int8_t power)
 204 {
 205         struct rc_proto_hello hdr;
 206         struct xbee_msg msg;
 207         uint8_t prio;
 208         int8_t ret;
 209
 210         hdr.type = RC_PROTO_HELLO;
 211         hdr.datalen = data_len;
 212
 213         msg.iovlen = 2;
 214         msg.iov[0].buf = &hdr;
 215         msg.iov[0].len = sizeof(hdr);
 216         msg.iov[1].buf = data;
 217         msg.iov[1].len = data_len;
 218
 219         /* set power level */
 220         if (power != -1)
 221                 xbeeapp_send_atcmd("PL", &power, sizeof(power), NULL, NULL);
 222
 223         /* we need to lock callout to increment stats */
 224         prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
 225         stats.hello_tx++;
 226         ret = xbeeapp_send_msg(addr, &msg, NULL, NULL);
 227         callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
 228
 229         return ret;
 230 }
 231
 232 /* send an echo message: expect a reply */
 233 int8_t rc_proto_send_echo_req(uint64_t addr, void *data, uint8_t data_len,
 234         int8_t power)
 235 {
 236         struct rc_proto_echo_req hdr;
 237         struct xbee_msg msg;
 238         uint8_t prio;
 239         int8_t ret;
 240
 241         hdr.type = RC_PROTO_ECHO_REQ;
 242         hdr.power = power;
 243         hdr.timestamp = get_time_ms();
 244         hdr.datalen = data_len;
 245
 246         msg.iovlen = 2;
 247         msg.iov[0].buf = &hdr;
 248         msg.iov[0].len = sizeof(hdr);
 249         msg.iov[1].buf = data;
 250         msg.iov[1].len = data_len;
 251
 252         /* set power level */
 253         if (power != -1)
 254                 xbeeapp_send_atcmd("PL", &power, sizeof(power), NULL, NULL);
 255
 256         /* we need to lock callout to increment stats */
 257         prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
 258         stats.echo_req_tx++;
 259         ret = xbeeapp_send_msg(addr, &msg, NULL, NULL);
 260         callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
 261
 262         return ret;
 263 }
 264
 265 /* send an echo message: expect a reply */
 266 int8_t rc_proto_send_echo_ans(uint64_t addr, void *data, uint8_t data_len,
 267         int8_t power)
 268 {
 269         struct rc_proto_echo_ans hdr;
 270         struct xbee_msg msg;
 271         uint8_t prio;
 272         int8_t ret;
 273
 274         hdr.type = RC_PROTO_ECHO_ANS;
 275         hdr.datalen = data_len;
 276
 277         msg.iovlen = 2;
 278         msg.iov[0].buf = &hdr;
 279         msg.iov[0].len = sizeof(hdr);
 280         msg.iov[1].buf = data;
 281         msg.iov[1].len = data_len;
 282
 283         /* set power level */
 284         if (power != -1)
 285                 xbeeapp_send_atcmd("PL", &power, sizeof(power), NULL, NULL);
 286
 287         /* we need to lock callout to increment stats */
 288         prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
 289         stats.echo_ans_tx++;
 290         ret = xbeeapp_send_msg(addr, &msg, NULL, NULL);
 291         callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
 292
 293         return ret;
 294 }
 295
 296 /* send an echo message: expect a reply */
 297 int8_t rc_proto_send_power_probe(uint64_t addr, uint8_t power)
 298 {
 299         struct rc_proto_power_probe hdr;
 300         struct xbee_msg msg;
 301         uint8_t prio;
 302         int8_t ret;
 303
 304         hdr.type = RC_PROTO_POWER_PROBE;
 305         hdr.power_level = power;
 306
 307         msg.iovlen = 1;
 308         msg.iov[0].buf = &hdr;
 309         msg.iov[0].len = sizeof(hdr);
 310
 311         /* set power level */
 312         xbeeapp_send_atcmd("PL", &power, sizeof(power), NULL, NULL);
 313
 314         /* we need to lock callout to increment stats */
 315         prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
 316         stats.power_probe_tx++;
 317         ret = xbeeapp_send_msg(addr, &msg, NULL, NULL);
 318         callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
 319
 320         return ret;
 321 }
 322
 323 /* convert values from servo_tx.servos into a xbee frame */
 324 static int8_t servo2buf(uint8_t buf[RC_PROTO_SERVO_LEN],
 325         uint8_t seq, uint8_t bypass, uint8_t pow, const uint16_t servos[N_SERVO])
 326 {
 327         uint8_t i = 0;
 328
 329         buf[i++] = RC_PROTO_SERVO;
 330         buf[i++] = ((seq & 0xf) << 4) | (bypass << 3) |  (pow & 0x7);
 331
 332         buf[i++] = servos[0] >> 2;
 333         buf[i] = (servos[0] & 0x3) << 6;
 334
 335         buf[i++] |= servos[1] >> 4;
 336         buf[i] = (servos[1] & 0xf) << 4;
 337
 338         buf[i++] |= servos[2] >> 6;
 339         buf[i] = (servos[2] & 0x3f) << 2;
 340
 341         buf[i++] |= servos[3] >> 8;
 342         buf[i++] = servos[3] & 0xff;
 343
 344         buf[i++] = servos[4] >> 2;
 345         buf[i] = (servos[4] & 0x3) << 6;
 346
 347         buf[i++] |= servos[5] >> 4;
 348         buf[i] = (servos[5] & 0xf) << 4;
 349
 350         return 0;
 351 }
 352
 353 /* send servos, called periodically with prio = XBEE_PRIO */
 354 static int8_t rc_proto_send_servos(void)
 355 {
 356         struct rc_proto_servo hdr;
 357         struct xbee_msg msg;
 358         uint8_t i, updated = 0;
 359         uint16_t ms, diff, servo_val;
 360         uint8_t frame[RC_PROTO_SERVO_LEN];
 361         int8_t ret;
 362         uint8_t power;
 363
 364         /* servo send disabled */
 365         if ((rc_proto_flags & RC_PROTO_FLAGS_TX_MASK) == RC_PROTO_FLAGS_TX_OFF)
 366                 return 0;
 367
 368         /* if we transmitted servos values recently, nothing to do */
 369         ms = get_time_ms();
 370         diff = ms - servo_tx.time;
 371         if (diff < rc_proto_timers.send_servo_min_ms)
 372                 return 0;
 373
 374         /* prepare values to send */
 375         if ((rc_proto_flags & RC_PROTO_FLAGS_TX_MASK) ==
 376                 RC_PROTO_FLAGS_TX_COPY_SPI) {
 377
 378                 /* set bypass to 0 */
 379                 if (servo_tx.bypass == 1) {
 380                         servo_tx.bypass = 0;
 381                         updated = 1;
 382                 }
 383
 384                 /* copy values from spi */
 385                 for (i = 0; i < N_SERVO; i++) {
 386                         servo_val = spi_servo_get(i);
 387                         if (servo_val != servo_tx.servos[i]) {
 388                                 servo_tx.servos[i] = servo_val;
 389                                 updated = 1;
 390                         }
 391                 }
 392         }
 393         else {
 394                 /* set bypass to 1 */
 395                 if (servo_tx.bypass == 0) {
 396                         servo_tx.bypass = 1;
 397                         updated = 1;
 398                 }
 399         }
 400
 401         /* if no value changed and last message is acknowledged, don't transmit
 402          * if we already transmitted quite recently */
 403         if (updated == 0 && ack == servo_tx.seq &&
 404                 diff < rc_proto_timers.send_servo_max_ms)
 405                 return 0;
 406
 407         /* ok, we need to transmit */
 408
 409         /* get the new seq value */
 410         if (updated == 1) {
 411                 servo_tx.seq++;
 412                 servo_tx.seq &= 0x1f;
 413                 if (servo_tx.seq == ack)
 414                         servo_tx.seq = (ack - 1) & 0x1f;
 415         }
 416         /* reset the "updated" flag and save time */
 417         servo_tx.time = ms;
 418
 419         /* set power level */
 420         power = get_best_power();
 421         xbeeapp_send_atcmd("PL", &power, sizeof(power), NULL, NULL);
 422
 423         /* create frame and send it */
 424         servo2buf(frame, servo_tx.seq, servo_tx.bypass, power, servo_tx.servos);
 425         hdr.type = RC_PROTO_SERVO;
 426
 427         msg.iovlen = 2;
 428         msg.iov[0].buf = &hdr;
 429         msg.iov[0].len = sizeof(hdr);
 430         msg.iov[1].buf = frame;
 431         msg.iov[1].len = RC_PROTO_SERVO_LEN;
 432
 433         stats.servo_tx++;
 434         ret = xbeeapp_send_msg(rc_proto_dstaddr, &msg, NULL, NULL);
 435         stats.servo_tx++;
 436
 437         return ret;
 438 }
 439
 440 /* send a ack message: no answer expected */
 441 int8_t rc_proto_send_ack(uint64_t addr, uint8_t seq, int8_t power)
 442 {
 443         struct rc_proto_ack hdr;
 444         struct xbee_msg msg;
 445         uint8_t prio;
 446         int8_t ret;
 447
 448         hdr.type = RC_PROTO_ACK;
 449         hdr.seq = seq;
 450
 451         msg.iovlen = 1;
 452         msg.iov[0].buf = &hdr;
 453         msg.iov[0].len = sizeof(hdr);
 454
 455         /* set power level */
 456         if (power != -1)
 457                 xbeeapp_send_atcmd("PL", &power, sizeof(power), NULL, NULL);
 458
 459         /* we need to lock callout to increment stats */
 460         prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
 461         stats.ack_tx++;
 462         ret = xbeeapp_send_msg(addr, &msg, NULL, NULL);
 463         callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
 464
 465         return ret;
 466 }
 467
 468 /* send a hello message: no answer expected */
 469 int8_t rc_proto_send_stats(uint64_t addr, int8_t power)
 470 {
 471         struct rc_proto_stats hdr;
 472         struct xbee_msg msg;
 473         uint8_t prio;
 474         int8_t ret;
 475
 476         hdr.type = RC_PROTO_STATS;
 477
 478         msg.iovlen = 2;
 479         msg.iov[0].buf = &hdr;
 480         msg.iov[0].len = sizeof(hdr);
 481         msg.iov[1].buf = &stats;
 482         msg.iov[1].len = sizeof(stats);
 483
 484         /* set power level */
 485         if (power != -1)
 486                 xbeeapp_send_atcmd("PL", &power, sizeof(power), NULL, NULL);
 487
 488         /* we need to lock callout to increment stats */
 489         prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
 490         stats.stats_tx++;
 491         ret = xbeeapp_send_msg(addr, &msg, NULL, NULL);
 492         callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
 493
 494         return ret;
 495 }
 496
 497 void rc_proto_set_mode(uint8_t flags)
 498 {
 499         rc_proto_flags = flags;
 500 }
 501
 502 uint8_t rc_proto_get_mode(void)
 503 {
 504         return rc_proto_flags;
 505 }
 506
 507 /* convert a receved servo frame into servo values */
 508 static int8_t buf2servo(uint16_t servos[N_SERVO], const uint8_t *buf)
 509 {
 510         uint16_t val;
 511
 512         val = buf[1];
 513         val <<= 2;
 514         val |= (buf[2] >> 6);
 515         servos[0] = val;
 516
 517         val = buf[2] & 0x3f;
 518         val <<= 4;
 519         val |= (buf[3] >> 4);
 520         servos[1] = val;
 521
 522         val = buf[3] & 0xf;
 523         val <<= 6;
 524         val |= (buf[4] >> 2);
 525         servos[2] = val;
 526
 527         val = buf[4] & 0x3;
 528         val <<= 8;
 529         val |= (buf[5]);
 530         servos[3] = val;
 531
 532         val = buf[6];
 533         val <<= 2;
 534         val |= (buf[7] >> 6);
 535         servos[4] = val;
 536
 537         val = buf[7];
 538         val <<= 4;
 539         val |= (buf[8] >> 4);
 540         servos[5] = val;
 541
 542         return 0;
 543 }
 544
 545 /* process a received servo frame */
 546 static int8_t rc_proto_rx_servo(struct rc_proto_servo *rcs)
 547 {
 548         uint8_t bypass;
 549         uint8_t i, seq, pow;
 550
 551         bypass = !!(rcs->data[0] & 0x08);
 552         pow = rcs->data[0] & 0x07;
 553
 554         /* convert it in a table of servo values */
 555         if (bypass == 0 && buf2servo(servo_rx.servos, rcs->data) < 0)
 556                 return -1;
 557
 558         /* save time */
 559         servo_rx.time = get_time_ms();
 560
 561         /* acknowledge received frame */
 562         seq = rcs->data[0] >> 4;
 563         rc_proto_send_ack(rc_proto_dstaddr, seq, pow);
 564
 565         /* copy values to spi */
 566         if (rc_proto_flags & RC_PROTO_FLAGS_RX_COPY_SPI) {
 567                 spi_servo_set_bypass(bypass);
 568
 569                 if (bypass == 0) {
 570                         for (i = 0; i < N_SERVO; i++)
 571                                 spi_servo_set(i, servo_rx.servos[i]);
 572                 }
 573         }
 574         return 0;
 575 }
 576
 577 /* receive a rc_proto message */
 578 int rc_proto_rx(struct xbee_recv_hdr *recvframe, unsigned len)
 579 {
 580         unsigned int datalen;
 581         struct rc_proto_hdr *rch = (struct rc_proto_hdr *) &recvframe->data;
 582
 583         if (len <  sizeof(*recvframe))
 584                 return -1;
 585
 586         datalen = len - sizeof(*recvframe);
 587         if (datalen < sizeof(struct rc_proto_hdr))
 588                 return -1;
 589
 590         /* other command types */
 591         switch (rch->type) {
 592                 case RC_PROTO_HELLO: {
 593                         struct rc_proto_hello *rch =
 594                                 (struct rc_proto_hello *) recvframe->data;
 595
 596                         NOTICE(E_USER_XBEE, "recv hello len=%d", rch->datalen);
 597                         stats.hello_rx++;
 598                         return 0;
 599                 }
 600
 601                 case RC_PROTO_ECHO_REQ: {
 602                         struct rc_proto_echo_req *rce =
 603                                 (struct rc_proto_echo_req *) recvframe->data;
 604                         int8_t power = rce->power;
 605
 606                         NOTICE(E_USER_XBEE, "recv echo len=%d", rce->datalen);
 607                         stats.echo_req_rx++;
 608
 609                         if (rc_proto_send_echo_ans(ntohll(recvframe->srcaddr),
 610                                         rce->data, rce->datalen, power) < 0)
 611                                 return -1;
 612
 613                         return 0;
 614                 }
 615
 616                 case RC_PROTO_ECHO_ANS: {
 617                         struct rc_proto_echo_ans *rce =
 618                                 (struct rc_proto_echo_ans *) recvframe->data;
 619                         uint16_t diff;
 620
 621                         NOTICE(E_USER_XBEE, "recv echo_ans len=%d", rce->datalen);
 622                         stats.echo_ans_rx++;
 623                         diff = get_time_ms() - rce->timestamp;
 624                         stats.echo_ans_latency_sum += diff;
 625                         return 0;
 626                 }
 627
 628                 /* received by the radio controller every ~500ms */
 629                 case RC_PROTO_POWER_PROBE: {
 630                         struct rc_proto_power_probe *rcpb =
 631                                 (struct rc_proto_power_probe *) recvframe->data;
 632
 633                         NOTICE(E_USER_XBEE, "recv power_probe");
 634
 635                         if (datalen != sizeof(*rcpb))
 636                                 return -1;
 637
 638                         if (rcpb->power_level >= MAX_POWER_LEVEL)
 639                                 return -1;
 640
 641                         stats.power_probe_rx++;
 642                         /* ask the DB value to the xbee module */
 643                         rc_proto_rx_power_probe(rcpb->power_level);
 644
 645                         return 0;
 646                 }
 647
 648                 /* received by the radio controller */
 649                 case RC_PROTO_ACK: {
 650                         struct rc_proto_ack *rca =
 651                                 (struct rc_proto_ack *) recvframe->data;
 652
 653                         NOTICE(E_USER_XBEE, "recv ack, ack=%d", rca->seq);
 654                         stats.ack_rx++;
 655                         return 0;
 656                 }
 657
 658                 /* received by the wing */
 659                 case RC_PROTO_SERVO: {
 660                         struct rc_proto_servo *rcs =
 661                                 (struct rc_proto_servo *) recvframe->data;
 662
 663                         NOTICE(E_USER_XBEE, "recv servo");
 664
 665                         if (datalen != RC_PROTO_SERVO_LEN)
 666                                 return -1;
 667
 668                         stats.servo_rx++;
 669                         return rc_proto_rx_servo(rcs);
 670                 }
 671
 672                 /* received by the radio controller */
 673                 case RC_PROTO_STATS: {
 674                         struct rc_proto_stats *rcs =
 675                                 (struct rc_proto_stats *) recvframe->data;
 676
 677                         NOTICE(E_USER_XBEE, "recv stats");
 678
 679                         if (datalen != sizeof(*rcs) + sizeof(peer_stats))
 680                                 return -1;
 681
 682                         stats.stats_rx++;
 683                         memcpy(&peer_stats, rcs->stats, sizeof(peer_stats));
 684                         return 0;
 685                 }
 686
 687                 default:
 688                         return -1;
 689         }
 690
 691         /* not reached */
 692         return 0;
 693 }
 694
 695 /* called by the scheduler, manage rc_proto periodical tasks */
 696 static void rc_proto_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
 697 {
 698         (void)arg;
 699         static uint16_t prev_stats_send;
 700         static uint16_t prev_compute_pow;
 701         static uint16_t prev_power_probe;
 702         static uint8_t pow_probe;
 703         uint16_t t, diff;
 704
 705         t = get_time_ms();
 706
 707         /* send servo values if flags are enabled. The function will decide
 708          * by itself if it's time to send or not */
 709         rc_proto_send_servos();
 710
 711         /* send power probe periodically */
 712         if (rc_proto_flags & RC_PROTO_FLAGS_TX_POW_PROBE) {
 713                 diff = t - prev_power_probe;
 714                 if (diff > rc_proto_timers.send_power_probe_ms) {
 715                         pow_probe++;
 716                         if (pow_probe > 4)
 717                                 pow_probe = 0;
 718                         rc_proto_send_power_probe(rc_proto_dstaddr, pow_probe);
 719                         prev_power_probe = t;
 720                 }
 721         }
 722
 723         /* on wing, auto bypass servos if no commands since some time */
 724         if (rc_proto_flags & RC_PROTO_FLAGS_RX_AUTOBYPASS) {
 725                 diff = t - servo_rx.time;
 726                 if (diff > rc_proto_timers.autobypass_ms)
 727                         spi_servo_set_bypass(1);
 728         }
 729
 730         /* send stats to peer every second */
 731         diff = t - prev_compute_pow;
 732         if (diff >= 1000) {
 733                 compute_best_power();
 734                 prev_compute_pow = t;
 735         }
 736
 737         /* send stats to peer every second */
 738         if (rc_proto_flags & RC_PROTO_FLAGS_TX_STATS) {
 739                 diff = t - prev_stats_send;
 740                 if (diff >= 1000) {
 741                         rc_proto_send_stats(rc_proto_dstaddr, get_best_power());
 742                         prev_stats_send = t;
 743                 }
 744         }
 745
 746         callout_schedule(cm, tim, 0);
 747 }
 748
 749 void rc_proto_dump_stats(void)
 750 {
 751         printf_P(PSTR("rc_proto stats LOCAL\r\n"));
 752         printf_P(PSTR("  hello_tx: %"PRIu32"\r\n"), stats.hello_tx);
 753         printf_P(PSTR("  hello_rx: %"PRIu32"\r\n"), stats.hello_rx);
 754         printf_P(PSTR("  echo_req_rx: %"PRIu32"\r\n"), stats.echo_req_rx);
 755         printf_P(PSTR("  echo_req_tx: %"PRIu32"\r\n"), stats.echo_req_tx);
 756         printf_P(PSTR("  echo_ans_rx: %"PRIu32"\r\n"), stats.echo_ans_rx);
 757         printf_P(PSTR("  echo_ans_tx: %"PRIu32"\r\n"), stats.echo_ans_tx);
 758         printf_P(PSTR("  power_probe_rx: %"PRIu32"\r\n"), stats.power_probe_rx);
 759         printf_P(PSTR("  power_probe_tx: %"PRIu32"\r\n"), stats.power_probe_tx);
 760         printf_P(PSTR("  ack_rx: %"PRIu32"\r\n"), stats.ack_rx);
 761         printf_P(PSTR("  ack_tx: %"PRIu32"\r\n"), stats.ack_tx);
 762         printf_P(PSTR("  servo_rx: %"PRIu32"\r\n"), stats.servo_rx);
 763         printf_P(PSTR("  servo_tx: %"PRIu32"\r\n"), stats.servo_tx);
 764         printf_P(PSTR("  stats_rx: %"PRIu32"\r\n"), stats.stats_rx);
 765         printf_P(PSTR("  stats_tx: %"PRIu32"\r\n"), stats.stats_tx);
 766         if (stats.echo_ans_rx != 0) {
 767                 printf_P(PSTR("  echo_ans_latency_ms: %"PRIu32"\r\n"),
 768                         stats.echo_ans_latency_sum / stats.echo_ans_rx);
 769         }
 770
 771         printf_P(PSTR("rc_proto stats PEER\r\n"));
 772         printf_P(PSTR("  hello_tx: %"PRIu32"\r\n"), peer_stats.hello_tx);
 773         printf_P(PSTR("  hello_rx: %"PRIu32"\r\n"), peer_stats.hello_rx);
 774         printf_P(PSTR("  echo_req_rx: %"PRIu32"\r\n"), peer_stats.echo_req_rx);
 775         printf_P(PSTR("  echo_req_tx: %"PRIu32"\r\n"), peer_stats.echo_req_tx);
 776         printf_P(PSTR("  echo_ans_rx: %"PRIu32"\r\n"), peer_stats.echo_ans_rx);
 777         printf_P(PSTR("  echo_ans_tx: %"PRIu32"\r\n"), peer_stats.echo_ans_tx);
 778         printf_P(PSTR("  power_probe_rx: %"PRIu32"\r\n"), peer_stats.power_probe_rx);
 779         printf_P(PSTR("  power_probe_tx: %"PRIu32"\r\n"), peer_stats.power_probe_tx);
 780         printf_P(PSTR("  ack_rx: %"PRIu32"\r\n"), peer_stats.ack_rx);
 781         printf_P(PSTR("  ack_tx: %"PRIu32"\r\n"), peer_stats.ack_tx);
 782         printf_P(PSTR("  servo_rx: %"PRIu32"\r\n"), peer_stats.servo_rx);
 783         printf_P(PSTR("  servo_tx: %"PRIu32"\r\n"), peer_stats.servo_tx);
 784         printf_P(PSTR("  stats_rx: %"PRIu32"\r\n"), peer_stats.stats_rx);
 785         printf_P(PSTR("  stats_tx: %"PRIu32"\r\n"), peer_stats.stats_tx);
 786         if (stats.echo_ans_rx != 0) {
 787                 printf_P(PSTR("  echo_ans_latency_ms: %"PRIu32"\r\n"),
 788                         peer_stats.echo_ans_latency_sum / peer_stats.echo_ans_rx);
 789         }
 790 }
 791
 792 void rc_proto_reset_stats(void)
 793 {
 794         uint8_t prio;
 795
 796         prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
 797         memset(&stats, 0, sizeof(stats));
 798         callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
 799 }
 800
 801 void rc_proto_dump_servos(void)
 802 {
 803         uint8_t i;
 804
 805         printf_P(PSTR("servo rx\r\n"));
 806         for (i = 0; i < N_SERVO; i++) {
 807                 printf_P(PSTR("  servo[%d] = %d\r\n"), i, servo_rx.servos[i]);
 808         }
 809         printf_P(PSTR("servo tx\r\n"));
 810         printf_P(PSTR("  bypass=%d\r\n"), servo_tx.bypass);
 811         printf_P(PSTR("  seq=%d\r\n"), servo_tx.seq);
 812         printf_P(PSTR("  time=%d\r\n"), servo_tx.time);
 813         for (i = 0; i < N_SERVO; i++) {
 814                 printf_P(PSTR("  servo[%d] = %d\r\n"), i, servo_tx.servos[i]);
 815         }
 816 }
 817
 818 void rc_proto_set_dstaddr(uint64_t addr)
 819 {
 820         uint8_t flags;
 821
 822         IRQ_LOCK(flags);
 823         rc_proto_dstaddr = addr;
 824         IRQ_UNLOCK(flags);
 825 }
 826
 827 uint64_t rc_proto_get_dstaddr(void)
 828 {
 829         uint64_t addr;
 830         uint8_t flags;
 831
 832         IRQ_LOCK(flags);
 833         addr = rc_proto_dstaddr;
 834         IRQ_UNLOCK(flags);
 835         return addr;
 836 }
 837
 838 void rc_proto_init(void)
 839 {
 840         callout_init(&rc_proto_timer, rc_proto_cb, NULL, XBEE_PRIO);
 841         callout_schedule(&xbeeboard.intr_cm, &rc_proto_timer, 0);
 842 }