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