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