+ /* set power level */
+ if (power != -1)
+ xbeeapp_send_atcmd("PL", &power, sizeof(power), NULL, NULL);
+
+ /* we need to lock callout to increment stats */
+ prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
+ stats.stats_tx++;
+ ret = xbeeapp_send_msg(addr, &msg, NULL, NULL);
+ callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
+
+ return ret;
+}
+
+void rc_proto_set_mode(uint8_t flags)
+{
+ rc_proto_flags = flags;
+}
+
+uint8_t rc_proto_get_mode(void)
+{
+ return rc_proto_flags;
+}
+
+/* convert a receved servo frame into servo values */
+static int8_t buf2servo(uint16_t servos[N_SERVO], const uint8_t *buf)
+{
+ uint16_t val;
+
+ val = buf[1];
+ val <<= 2;
+ val |= (buf[2] >> 6);
+ servos[0] = val;
+
+ val = buf[2] & 0x3f;
+ val <<= 4;
+ val |= (buf[3] >> 4);
+ servos[1] = val;
+
+ val = buf[3] & 0xf;
+ val <<= 6;
+ val |= (buf[4] >> 2);
+ servos[2] = val;
+
+ val = buf[4] & 0x3;
+ val <<= 8;
+ val |= (buf[5]);
+ servos[3] = val;
+
+ val = buf[6];
+ val <<= 2;
+ val |= (buf[7] >> 6);
+ servos[4] = val;
+
+ val = buf[7];
+ val <<= 4;
+ val |= (buf[8] >> 4);
+ servos[5] = val;
+
+ return 0;
+}
+
+/* process a received servo frame */
+static int8_t rc_proto_rx_servo(struct rc_proto_servo *rcs)
+{
+ uint8_t bypass;
+ uint8_t i, seq, pow;
+
+ bypass = !!(rcs->data[0] & 0x08);
+ pow = rcs->data[0] & 0x07;
+
+ /* convert it in a table of servo values */
+ if (bypass == 0 && buf2servo(servo_rx.servos, rcs->data) < 0)
+ return -1;
+
+ /* save time */
+ servo_rx.time = get_time_ms();
+
+ /* acknowledge received frame */
+ seq = rcs->data[0] >> 4;
+ rc_proto_send_ack(rc_proto_dstaddr, seq, pow);
+
+ /* copy values to spi */
+ if (rc_proto_flags & RC_PROTO_FLAGS_RX_COPY_SPI) {
+ spi_servo_set_bypass(bypass);
+
+ if (bypass == 0) {
+ for (i = 0; i < N_SERVO; i++)
+ spi_servo_set(i, servo_rx.servos[i]);
+ }
+ }
+ return 0;
+}
+
+/* receive a rc_proto message */
+int rc_proto_rx(struct xbee_recv_hdr *recvframe, unsigned len)
+{
+ unsigned int datalen;
+ struct rc_proto_hdr *rch = (struct rc_proto_hdr *) &recvframe->data;
+
+ if (len < sizeof(*recvframe))
+ return -1;
+
+ datalen = len - sizeof(*recvframe);
+ if (datalen < sizeof(struct rc_proto_hdr))
+ return -1;
+
+ /* other command types */
+ switch (rch->type) {
+ case RC_PROTO_HELLO: {
+ struct rc_proto_hello *rch =
+ (struct rc_proto_hello *) recvframe->data;
+
+ NOTICE(E_USER_XBEE, "recv hello len=%d", rch->datalen);
+ stats.hello_rx++;
+ return 0;
+ }
+
+ case RC_PROTO_ECHO_REQ: {
+ struct rc_proto_echo_req *rce =
+ (struct rc_proto_echo_req *) recvframe->data;
+ int8_t power = rce->power;
+
+ NOTICE(E_USER_XBEE, "recv echo len=%d", rce->datalen);
+ stats.echo_req_rx++;
+
+ if (rc_proto_send_echo_ans(ntohll(recvframe->srcaddr),
+ rce->data, rce->datalen, power,
+ rce->timestamp) < 0)
+ return -1;
+
+ return 0;
+ }
+
+ case RC_PROTO_ECHO_ANS: {
+ struct rc_proto_echo_ans *rce =
+ (struct rc_proto_echo_ans *) recvframe->data;
+ uint16_t diff;
+
+ NOTICE(E_USER_XBEE, "recv echo_ans len=%d", rce->datalen);
+ stats.echo_ans_rx++;
+ diff = get_time_ms() - rce->timestamp;
+ stats.echo_ans_latency_sum += diff;
+ return 0;
+ }
+
+ /* received by the radio controller every ~500ms */
+ case RC_PROTO_POWER_PROBE: {
+ struct rc_proto_power_probe *rcpb =
+ (struct rc_proto_power_probe *) recvframe->data;
+
+ NOTICE(E_USER_XBEE, "recv power_probe");
+
+ if (datalen != sizeof(*rcpb))
+ return -1;
+
+ if (rcpb->power_level >= MAX_POWER_LEVEL)
+ return -1;
+
+ stats.power_probe_rx++;
+ /* ask the DB value to the xbee module */
+ rc_proto_rx_power_probe(rcpb->power_level);
+
+ return 0;
+ }
+
+ /* received by the radio controller */
+ case RC_PROTO_ACK: {
+ struct rc_proto_ack *rca =
+ (struct rc_proto_ack *) recvframe->data;
+
+ NOTICE(E_USER_XBEE, "recv ack, ack=%d", rca->seq);
+ stats.ack_rx++;
+ return 0;
+ }
+
+ /* received by the wing */
+ case RC_PROTO_SERVO: {
+ struct rc_proto_servo *rcs =
+ (struct rc_proto_servo *) recvframe->data;
+
+ NOTICE(E_USER_XBEE, "recv servo");
+
+ if (datalen != RC_PROTO_SERVO_LEN)
+ return -1;
+
+ stats.servo_rx++;
+ return rc_proto_rx_servo(rcs);
+ }
+
+ /* received by the radio controller */
+ case RC_PROTO_STATS: {
+ struct rc_proto_stats *rcs =
+ (struct rc_proto_stats *) recvframe->data;
+
+ NOTICE(E_USER_XBEE, "recv stats");
+
+ if (datalen != sizeof(*rcs) + sizeof(peer_stats))
+ return -1;
+
+ stats.stats_rx++;
+ memcpy(&peer_stats, rcs->stats, sizeof(peer_stats));
+ return 0;
+ }
+
+ default:
+ return -1;
+ }
+
+ /* not reached */
+ return 0;
+}
+
+/* called by the scheduler, manage rc_proto periodical tasks */
+static void rc_proto_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
+{
+ (void)arg;
+ static uint16_t prev_stats_send;
+ static uint16_t prev_compute_pow;
+ static uint16_t prev_power_probe;
+ static uint8_t pow_probe;
+ uint16_t t, diff;
+
+ t = get_time_ms();
+
+ /* send servo values if flags are enabled. The function will decide
+ * by itself if it's time to send or not */
+ rc_proto_send_servos();
+
+ /* send power probe periodically */
+ if (rc_proto_flags & RC_PROTO_FLAGS_TX_POW_PROBE) {
+ diff = t - prev_power_probe;
+ if (diff > rc_proto_timers.send_power_probe_ms) {
+ pow_probe++;
+ if (pow_probe > 4)
+ pow_probe = 0;
+ rc_proto_send_power_probe(rc_proto_dstaddr, pow_probe);
+ prev_power_probe = t;
+ }
+ }
+
+ /* on wing, auto bypass servos if no commands since some time */
+ if (rc_proto_flags & RC_PROTO_FLAGS_RX_AUTOBYPASS) {
+ diff = t - servo_rx.time;
+ if (diff > rc_proto_timers.autobypass_ms)
+ spi_servo_set_bypass(1);
+ }
+
+ /* send stats to peer every second */
+ if (rc_proto_flags & RC_PROTO_FLAGS_COMPUTE_BEST_POW) {
+ diff = t - prev_compute_pow;
+ if (diff >= 1000) {
+ compute_best_power();
+ prev_compute_pow = t;
+ }
+ }
+
+ /* send stats to peer every second */
+ if (rc_proto_flags & RC_PROTO_FLAGS_TX_STATS) {
+ diff = t - prev_stats_send;
+ if (diff >= 1000) {
+ rc_proto_send_stats(rc_proto_dstaddr, get_best_power());
+ prev_stats_send = t;
+ }
+ }
+
+ callout_schedule(cm, tim, 0);
+}
+
+void rc_proto_dump_stats(void)
+{
+ printf_P(PSTR("rc_proto stats LOCAL\r\n"));
+ printf_P(PSTR(" hello_tx: %"PRIu32"\r\n"), stats.hello_tx);
+ printf_P(PSTR(" hello_rx: %"PRIu32"\r\n"), stats.hello_rx);
+ printf_P(PSTR(" echo_req_rx: %"PRIu32"\r\n"), stats.echo_req_rx);
+ printf_P(PSTR(" echo_req_tx: %"PRIu32"\r\n"), stats.echo_req_tx);
+ printf_P(PSTR(" echo_ans_rx: %"PRIu32"\r\n"), stats.echo_ans_rx);
+ printf_P(PSTR(" echo_ans_tx: %"PRIu32"\r\n"), stats.echo_ans_tx);
+ printf_P(PSTR(" power_probe_rx: %"PRIu32"\r\n"), stats.power_probe_rx);
+ printf_P(PSTR(" power_probe_tx: %"PRIu32"\r\n"), stats.power_probe_tx);
+ printf_P(PSTR(" ack_rx: %"PRIu32"\r\n"), stats.ack_rx);
+ printf_P(PSTR(" ack_tx: %"PRIu32"\r\n"), stats.ack_tx);
+ printf_P(PSTR(" servo_rx: %"PRIu32"\r\n"), stats.servo_rx);
+ printf_P(PSTR(" servo_tx: %"PRIu32"\r\n"), stats.servo_tx);
+ printf_P(PSTR(" stats_rx: %"PRIu32"\r\n"), stats.stats_rx);
+ printf_P(PSTR(" stats_tx: %"PRIu32"\r\n"), stats.stats_tx);
+ if (stats.echo_ans_rx != 0) {
+ printf_P(PSTR(" echo_ans_latency_ms: %"PRIu32"\r\n"),
+ stats.echo_ans_latency_sum / stats.echo_ans_rx);
+ }
+
+ printf_P(PSTR("rc_proto stats PEER\r\n"));
+ printf_P(PSTR(" hello_tx: %"PRIu32"\r\n"), peer_stats.hello_tx);
+ printf_P(PSTR(" hello_rx: %"PRIu32"\r\n"), peer_stats.hello_rx);
+ printf_P(PSTR(" echo_req_rx: %"PRIu32"\r\n"), peer_stats.echo_req_rx);
+ printf_P(PSTR(" echo_req_tx: %"PRIu32"\r\n"), peer_stats.echo_req_tx);
+ printf_P(PSTR(" echo_ans_rx: %"PRIu32"\r\n"), peer_stats.echo_ans_rx);
+ printf_P(PSTR(" echo_ans_tx: %"PRIu32"\r\n"), peer_stats.echo_ans_tx);
+ printf_P(PSTR(" power_probe_rx: %"PRIu32"\r\n"), peer_stats.power_probe_rx);
+ printf_P(PSTR(" power_probe_tx: %"PRIu32"\r\n"), peer_stats.power_probe_tx);
+ printf_P(PSTR(" ack_rx: %"PRIu32"\r\n"), peer_stats.ack_rx);
+ printf_P(PSTR(" ack_tx: %"PRIu32"\r\n"), peer_stats.ack_tx);
+ printf_P(PSTR(" servo_rx: %"PRIu32"\r\n"), peer_stats.servo_rx);
+ printf_P(PSTR(" servo_tx: %"PRIu32"\r\n"), peer_stats.servo_tx);
+ printf_P(PSTR(" stats_rx: %"PRIu32"\r\n"), peer_stats.stats_rx);
+ printf_P(PSTR(" stats_tx: %"PRIu32"\r\n"), peer_stats.stats_tx);
+ if (peer_stats.echo_ans_rx != 0) {
+ printf_P(PSTR(" echo_ans_latency_ms: %"PRIu32"\r\n"),
+ peer_stats.echo_ans_latency_sum / peer_stats.echo_ans_rx);
+ }
+}
+
+void rc_proto_reset_stats(void)
+{
+ uint8_t prio;
+
+ prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
+ memset(&stats, 0, sizeof(stats));
+ callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
+}
+
+void rc_proto_dump_servos(void)
+{
+ uint8_t i;
+
+ printf_P(PSTR("servo rx\r\n"));
+ for (i = 0; i < N_SERVO; i++) {
+ printf_P(PSTR(" servo[%d] = %d\r\n"), i, servo_rx.servos[i]);
+ }
+ printf_P(PSTR("servo tx\r\n"));
+ printf_P(PSTR(" bypass=%d\r\n"), servo_tx.bypass);
+ printf_P(PSTR(" seq=%d\r\n"), servo_tx.seq);
+ printf_P(PSTR(" time=%d\r\n"), servo_tx.time);
+ for (i = 0; i < N_SERVO; i++) {
+ printf_P(PSTR(" servo[%d] = %d\r\n"), i, servo_tx.servos[i]);
+ }
+}
+
+void rc_proto_set_dstaddr(uint64_t addr)
+{
+ uint8_t flags;
+
+ IRQ_LOCK(flags);
+ rc_proto_dstaddr = addr;
+ IRQ_UNLOCK(flags);
+}
+
+uint64_t rc_proto_get_dstaddr(void)
+{
+ uint64_t addr;
+ uint8_t flags;
+
+ IRQ_LOCK(flags);
+ addr = rc_proto_dstaddr;
+ IRQ_UNLOCK(flags);
+ return addr;
+}
+
+void rc_proto_init(void)
+{
+ callout_init(&rc_proto_timer, rc_proto_cb, NULL, XBEE_PRIO);
+ callout_schedule(&xbeeboard.intr_cm, &rc_proto_timer, 0);
+}