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