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