2 * Copyright (c) 2013, Olivier MATZ <zer0@droids-corp.org>
4 * Redistribution and use in source and binary forms, with or without
5 * modification, are permitted provided that the following conditions are met:
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.
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.
31 #include <aversive/queue.h>
32 #include <aversive/endian.h>
45 #include "xbee_user.h"
46 #include "spi_servo.h"
47 #include "../common/i2c_commands.h"
48 #include "i2c_protocol.h"
52 #define SERVO_NBITS 10
54 #define RX_DB_THRESHOLD 65 /* mean -65 dB */
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,
66 /* rc_proto statistics, accessed with sched_prio=XBEE_PRIO */
67 struct rc_proto_stats_data {
74 uint32_t power_probe_rx;
75 uint32_t power_probe_tx;
86 uint32_t echo_ans_latency_sum;
88 static struct rc_proto_stats_data stats; /* local stats */
89 static struct rc_proto_stats_data peer_stats; /* peer stats */
91 /* store last received power probes */
92 struct rc_proto_power_levels {
96 static struct rc_proto_power_levels power_levels[MAX_POWER_LEVEL];
98 /* address of the peer */
99 static uint64_t rc_proto_dstaddr = 0xFFFF; /* broadcast by default */
101 /* state sent to the xbee peer (used on RC) */
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 */
108 static struct servo_tx servo_tx;
110 /* state received from the xbee peer (used on WING) */
112 uint16_t servos[N_SERVO];
113 uint16_t time; /* time of last xmit */
115 static struct servo_rx servo_rx;
117 /* the received seq value (acknowledged by the wing, received on the rc) */
120 /* define tx mode (disabled, send from spi, bypass), rx mode (auto-bypass),
122 static uint16_t rc_proto_flags;
124 /* callout managing rc_proto (ex: sending of servos periodically) */
125 static struct callout rc_proto_timer;
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;
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,
136 struct xbee_atresp_hdr *atresp = (struct xbee_atresp_hdr *)frame;
137 int level = (intptr_t)arg;
140 /* nothing more to do, error is already logged by xbee_user */
144 if (len < sizeof(struct xbee_atresp_hdr) + sizeof(uint8_t))
147 db = atresp->data[0];
148 power_levels[level].power_db = db;
149 power_levels[level].ttl = 10; /* valid during 10 seconds */
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)
156 xbeeapp_send_atcmd("DB", NULL, 0, update_power_level,
157 (void *)(intptr_t)power_level);
160 /* called every second to decide which power should be used */
161 static void compute_best_power(void)
163 int8_t best_power_level = -1;
167 for (i = 0; i < MAX_POWER_LEVEL; i++) {
168 if (power_levels[i].ttl > 0)
169 power_levels[i].ttl--;
172 for (i = 0; i < MAX_POWER_LEVEL; i++) {
173 if (power_levels[i].ttl == 0)
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;
183 /* we have no info, don't touch the negative value */
184 if (best_power_level < 0 && power_level_global < 0)
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);
191 power_level_global = best_power_level;
194 /* return the best power level, or -1 if best power level computation is
196 static int8_t get_best_power(void)
198 if ((rc_proto_flags & RC_PROTO_FLAGS_COMPUTE_BEST_POW) == 0)
202 if (power_level_global == -1) {
203 power_level_global = -4;
206 else if (power_level_global == -4) {
207 power_level_global = -1;
211 return power_level_global;
214 /* send a hello message: no answer expected */
215 int8_t rc_proto_send_hello(uint64_t addr, void *data, uint8_t data_len,
218 struct rc_proto_hello hdr;
223 hdr.type = RC_PROTO_HELLO;
224 hdr.datalen = data_len;
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;
232 /* set power level */
234 xbeeapp_send_atcmd("PL", &power, sizeof(power), NULL, NULL);
236 /* we need to lock callout to increment stats */
237 prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
239 ret = xbeeapp_send_msg(addr, &msg, NULL, NULL);
240 callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
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,
249 struct rc_proto_echo_req hdr;
254 hdr.type = RC_PROTO_ECHO_REQ;
256 hdr.timestamp = get_time_ms();
257 hdr.datalen = data_len;
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;
265 /* set power level */
267 xbeeapp_send_atcmd("PL", &power, sizeof(power), NULL, NULL);
269 /* we need to lock callout to increment stats */
270 prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
272 ret = xbeeapp_send_msg(addr, &msg, NULL, NULL);
273 callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
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)
282 struct rc_proto_echo_ans hdr;
287 hdr.type = RC_PROTO_ECHO_ANS;
288 hdr.datalen = data_len;
289 hdr.timestamp = timestamp;
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;
297 /* set power level */
299 xbeeapp_send_atcmd("PL", &power, sizeof(power), NULL, NULL);
301 /* we need to lock callout to increment stats */
302 prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
304 ret = xbeeapp_send_msg(addr, &msg, NULL, NULL);
305 callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
310 /* send an echo message: expect a reply */
311 int8_t rc_proto_send_power_probe(uint64_t addr, uint8_t power)
313 struct rc_proto_power_probe hdr;
318 hdr.type = RC_PROTO_POWER_PROBE;
319 hdr.power_level = power;
322 msg.iov[0].buf = &hdr;
323 msg.iov[0].len = sizeof(hdr);
325 /* set power level */
326 xbeeapp_send_atcmd("PL", &power, sizeof(power), NULL, NULL);
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);
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])
343 buf[i++] = RC_PROTO_SERVO;
344 buf[i++] = ((seq & 0xf) << 4) | (bypass << 3) | (pow & 0x7);
346 buf[i++] = servos[0] >> 2;
347 buf[i] = (servos[0] & 0x3) << 6;
349 buf[i++] |= servos[1] >> 4;
350 buf[i] = (servos[1] & 0xf) << 4;
352 buf[i++] |= servos[2] >> 6;
353 buf[i] = (servos[2] & 0x3f) << 2;
355 buf[i++] |= servos[3] >> 8;
356 buf[i++] = servos[3] & 0xff;
358 buf[i++] = servos[4] >> 2;
359 buf[i] = (servos[4] & 0x3) << 6;
361 buf[i++] |= servos[5] >> 4;
362 buf[i] = (servos[5] & 0xf) << 4;
367 /* send servos, called periodically with prio = XBEE_PRIO */
368 static int8_t rc_proto_send_servos(void)
370 struct rc_proto_servo hdr;
372 uint8_t i, updated = 0;
373 uint16_t ms, diff, servo_val;
374 uint8_t frame[RC_PROTO_SERVO_LEN];
378 /* servo send disabled */
379 if ((rc_proto_flags & RC_PROTO_FLAGS_TX_MASK) == RC_PROTO_FLAGS_TX_OFF)
382 /* if we transmitted servos values recently, nothing to do */
384 diff = ms - servo_tx.time;
385 if (diff < rc_proto_timers.send_servo_min_ms)
388 /* prepare values to send */
389 if ((rc_proto_flags & RC_PROTO_FLAGS_TX_MASK) ==
390 RC_PROTO_FLAGS_TX_COPY_SPI) {
392 /* set bypass to 0 */
393 if (servo_tx.bypass == 1) {
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;
408 /* set bypass to 1 */
409 if (servo_tx.bypass == 0) {
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)
421 /* ok, we need to transmit */
423 /* get the new seq value */
426 servo_tx.seq &= 0x1f;
427 if (servo_tx.seq == ack)
428 servo_tx.seq = (ack - 1) & 0x1f;
430 /* reset the "updated" flag and save time */
433 /* set power level */
434 power = get_best_power();
435 xbeeapp_send_atcmd("PL", &power, sizeof(power), NULL, NULL);
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;
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;
448 ret = xbeeapp_send_msg(rc_proto_dstaddr, &msg, NULL, NULL);
454 /* send a ack message: no answer expected */
455 int8_t rc_proto_send_ack(uint64_t addr, uint8_t seq, int8_t power)
457 struct rc_proto_ack hdr;
462 hdr.type = RC_PROTO_ACK;
466 msg.iov[0].buf = &hdr;
467 msg.iov[0].len = sizeof(hdr);
469 /* set power level */
471 xbeeapp_send_atcmd("PL", &power, sizeof(power), NULL, NULL);
473 /* we need to lock callout to increment stats */
474 prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
476 ret = xbeeapp_send_msg(addr, &msg, NULL, NULL);
477 callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
482 /* send a hello message: no answer expected */
483 int8_t rc_proto_send_stats(uint64_t addr, int8_t power)
485 struct rc_proto_stats hdr;
490 hdr.type = RC_PROTO_STATS;
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);
498 /* set power level */
500 xbeeapp_send_atcmd("PL", &power, sizeof(power), NULL, NULL);
502 /* we need to lock callout to increment stats */
503 prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
505 ret = xbeeapp_send_msg(addr, &msg, NULL, NULL);
506 callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
511 /* send a gps_pos message: no answer expected */
512 int8_t rc_proto_send_gps_pos(uint64_t addr, int8_t power)
514 struct rc_proto_gps_pos gps_msg;
520 gps_msg.type = RC_PROTO_GPS_POS;
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);
530 msg.iov[0].buf = &gps_msg;
531 msg.iov[0].len = sizeof(gps_msg);
533 /* set power level */
535 xbeeapp_send_atcmd("PL", &power, sizeof(power), NULL, NULL);
537 /* we also need to lock callout to increment stats */
538 prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
540 ret = xbeeapp_send_msg(addr, &msg, NULL, NULL);
541 callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
546 /* send a imu_pos message: no answer expected */
547 int8_t rc_proto_send_imu_pos(uint64_t addr, int8_t power)
549 struct rc_proto_imu_pos imu_msg;
555 imu_msg.type = RC_PROTO_IMU_POS;
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);
565 msg.iov[0].buf = &imu_msg;
566 msg.iov[0].len = sizeof(imu_msg);
568 /* set power level */
570 xbeeapp_send_atcmd("PL", &power, sizeof(power), NULL, NULL);
572 /* we also need to lock callout to increment stats */
573 prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
575 ret = xbeeapp_send_msg(addr, &msg, NULL, NULL);
576 callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
583 void rc_proto_set_mode(uint16_t flags)
585 rc_proto_flags = flags;
588 uint16_t rc_proto_get_mode(void)
590 return rc_proto_flags;
593 /* convert a receved servo frame into servo values */
594 static int8_t buf2servo(uint16_t servos[N_SERVO], const uint8_t *buf)
600 val |= (buf[2] >> 6);
605 val |= (buf[3] >> 4);
610 val |= (buf[4] >> 2);
620 val |= (buf[7] >> 6);
625 val |= (buf[8] >> 4);
631 /* process a received servo frame */
632 static int8_t rc_proto_rx_servo(struct rc_proto_servo *rcs)
637 bypass = !!(rcs->data[0] & 0x08);
638 pow = rcs->data[0] & 0x07;
640 /* convert it in a table of servo values */
641 if (bypass == 0 && buf2servo(servo_rx.servos, rcs->data) < 0)
645 servo_rx.time = get_time_ms();
647 /* acknowledge received frame */
648 seq = rcs->data[0] >> 4;
649 rc_proto_send_ack(rc_proto_dstaddr, seq, pow);
651 /* copy values to spi */
652 if (rc_proto_flags & RC_PROTO_FLAGS_RX_COPY_SPI) {
653 spi_servo_set_bypass(bypass);
656 for (i = 0; i < N_SERVO; i++)
657 spi_servo_set(i, servo_rx.servos[i]);
663 /* receive a rc_proto message */
664 int rc_proto_rx(struct xbee_recv_hdr *recvframe, unsigned len)
666 unsigned int datalen;
667 struct rc_proto_hdr *rch = (struct rc_proto_hdr *) &recvframe->data;
669 if (len < sizeof(*recvframe))
672 datalen = len - sizeof(*recvframe);
673 if (datalen < sizeof(struct rc_proto_hdr))
676 /* other command types */
678 case RC_PROTO_HELLO: {
679 struct rc_proto_hello *rch =
680 (struct rc_proto_hello *) recvframe->data;
682 NOTICE(E_USER_XBEE, "recv hello len=%d", rch->datalen);
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;
692 NOTICE(E_USER_XBEE, "recv echo len=%d", rce->datalen);
695 if (rc_proto_send_echo_ans(ntohll(recvframe->srcaddr),
696 rce->data, rce->datalen, power,
703 case RC_PROTO_ECHO_ANS: {
704 struct rc_proto_echo_ans *rce =
705 (struct rc_proto_echo_ans *) recvframe->data;
708 NOTICE(E_USER_XBEE, "recv echo_ans len=%d", rce->datalen);
710 diff = get_time_ms() - rce->timestamp;
711 stats.echo_ans_latency_sum += diff;
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;
720 NOTICE(E_USER_XBEE, "recv power_probe");
722 if (datalen != sizeof(*rcpb))
725 if (rcpb->power_level >= MAX_POWER_LEVEL)
728 stats.power_probe_rx++;
729 /* ask the DB value to the xbee module */
730 rc_proto_rx_power_probe(rcpb->power_level);
735 /* received by the radio controller */
737 struct rc_proto_ack *rca =
738 (struct rc_proto_ack *) recvframe->data;
740 NOTICE(E_USER_XBEE, "recv ack, ack=%d", rca->seq);
745 /* received by the wing */
746 case RC_PROTO_SERVO: {
747 struct rc_proto_servo *rcs =
748 (struct rc_proto_servo *) recvframe->data;
750 NOTICE(E_USER_XBEE, "recv servo");
752 if (datalen != RC_PROTO_SERVO_LEN)
756 return rc_proto_rx_servo(rcs);
759 /* received by the radio controller */
760 case RC_PROTO_STATS: {
761 struct rc_proto_stats *rcs =
762 (struct rc_proto_stats *) recvframe->data;
764 NOTICE(E_USER_XBEE, "recv stats");
766 if (datalen != sizeof(*rcs) + sizeof(peer_stats))
770 memcpy(&peer_stats, rcs->stats, sizeof(peer_stats));
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;
779 NOTICE(E_USER_XBEE, "recv gps_pos");
781 if (datalen != sizeof(*rcmsg))
785 printf_P(PSTR("GPS received valid=%d %"PRIu32" %"
786 PRIu32" %"PRIu32"\n"),
787 rcmsg->valid, rcmsg->latitude, rcmsg->longitude,
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;
797 NOTICE(E_USER_XBEE, "recv imu_pos");
799 if (datalen != sizeof(*rcmsg))
803 printf_P(PSTR("IMU received %d %d %d\n"),
804 rcmsg->roll, rcmsg->pitch, rcmsg->yaw);
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)
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;
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();
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) {
841 rc_proto_send_power_probe(rc_proto_dstaddr, pow_probe);
842 prev_power_probe = t;
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) {
853 rc_proto_send_gps_pos(rc_proto_dstaddr, pow_probe);
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) {
865 rc_proto_send_imu_pos(rc_proto_dstaddr, pow_probe);
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);
877 /* send stats to peer every second */
878 if (rc_proto_flags & RC_PROTO_FLAGS_COMPUTE_BEST_POW) {
879 diff = t - prev_compute_pow;
881 compute_best_power();
882 prev_compute_pow = t;
886 /* send stats to peer every second */
887 if (rc_proto_flags & RC_PROTO_FLAGS_TX_STATS) {
888 diff = t - prev_stats_send;
890 rc_proto_send_stats(rc_proto_dstaddr, get_best_power());
895 callout_schedule(cm, tim, 0);
898 static void __dump_stats(struct rc_proto_stats_data *s)
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);
924 void rc_proto_dump_stats(void)
926 printf_P(PSTR("rc_proto stats LOCAL\r\n"));
927 __dump_stats(&stats);
929 printf_P(PSTR("rc_proto stats PEER\r\n"));
930 __dump_stats(&peer_stats);
933 void rc_proto_reset_stats(void)
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);
942 void rc_proto_dump_servos(void)
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]);
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]);
959 void rc_proto_set_dstaddr(uint64_t addr)
964 rc_proto_dstaddr = addr;
968 uint64_t rc_proto_get_dstaddr(void)
974 addr = rc_proto_dstaddr;
979 void rc_proto_init(void)
981 callout_init(&rc_proto_timer, rc_proto_cb, NULL, XBEE_PRIO);
982 callout_schedule(&xbeeboard.intr_cm, &rc_proto_timer, 0);