+ power_levels[level].ttl = 10; /* valid during 10 seconds */
+ return 0;
+}
+
+/* when we receive a power probe, ask the DB value to the xbee */
+static void rc_proto_rx_power_probe(int power_level)
+{
+ xbeeapp_send_atcmd("DB", NULL, 0, update_power_level,
+ (void *)(intptr_t)power_level);
+}
+
+/* called every second to decide which power should be used */
+static void compute_best_power(void)
+{
+ int8_t best_power_level = -1;
+ int8_t i;
+
+ /* decrement TTL */
+ for (i = 0; i < MAX_POWER_LEVEL; i++) {
+ if (power_levels[i].ttl > 0)
+ power_levels[i].ttl--;
+ }
+
+ for (i = 0; i < MAX_POWER_LEVEL; i++) {
+ if (power_levels[i].ttl == 0)
+ continue;
+
+ /* if signal is powerful enough, select this as level */
+ if (power_levels[i].power_db < RX_DB_THRESHOLD) {
+ best_power_level = i;
+ break;
+ }
+ }
+
+ /* we have no info, don't touch the negative value */
+ if (best_power_level < 0 && power_level_global < 0)
+ return;
+
+ if (power_level_global != best_power_level) {
+ DEBUG(E_USER_RC_PROTO, "changing power level %d => %d\n",
+ power_level_global, best_power_level);
+ }
+ power_level_global = best_power_level;
+}
+
+/* return the best power level, or -1 if best power level computation is
+ * disabled. */
+static int8_t get_best_power(void)
+{
+ if ((rc_proto_flags & RC_PROTO_FLAGS_COMPUTE_BEST_POW) == 0)
+ return -1;
+
+ /* special values */
+ if (power_level_global == -1) {
+ power_level_global = -4;
+ return 4;
+ }
+ else if (power_level_global == -4) {
+ power_level_global = -1;
+ return 0;
+ }
+ else
+ return power_level_global;
+}
+
+/* send a hello message: no answer expected */
+int8_t rc_proto_send_hello(uint64_t addr, void *data, uint8_t data_len,
+ int8_t power)
+{
+ struct rc_proto_hello hdr;
+ struct xbee_msg msg;
+ uint8_t prio;
+ int8_t ret;
+
+ hdr.type = RC_PROTO_HELLO;
+ hdr.datalen = data_len;
+
+ msg.iovlen = 2;
+ msg.iov[0].buf = &hdr;
+ msg.iov[0].len = sizeof(hdr);
+ msg.iov[1].buf = data;
+ msg.iov[1].len = data_len;
+
+ /* 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.hello_tx++;
+ ret = xbeeapp_send_msg(addr, &msg, NULL, NULL);
+ callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
+
+ return ret;
+}
+
+/* send an echo message: expect a reply */
+int8_t rc_proto_send_echo_req(uint64_t addr, void *data, uint8_t data_len,
+ int8_t power)
+{
+ struct rc_proto_echo_req hdr;
+ struct xbee_msg msg;
+ uint8_t prio;
+ int8_t ret;
+
+ hdr.type = RC_PROTO_ECHO_REQ;
+ hdr.power = power;
+ hdr.timestamp = get_time_ms();
+ hdr.datalen = data_len;
+
+ msg.iovlen = 2;
+ msg.iov[0].buf = &hdr;
+ msg.iov[0].len = sizeof(hdr);
+ msg.iov[1].buf = data;
+ msg.iov[1].len = data_len;
+
+ /* 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.echo_req_tx++;
+ ret = xbeeapp_send_msg(addr, &msg, NULL, NULL);
+ callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
+
+ return ret;
+}
+
+/* send an echo message: expect a reply */
+int8_t rc_proto_send_echo_ans(uint64_t addr, void *data, uint8_t data_len,
+ int8_t power, uint16_t timestamp)
+{
+ struct rc_proto_echo_ans hdr;
+ struct xbee_msg msg;
+ uint8_t prio;
+ int8_t ret;
+
+ hdr.type = RC_PROTO_ECHO_ANS;
+ hdr.datalen = data_len;
+ hdr.timestamp = timestamp;
+
+ msg.iovlen = 2;
+ msg.iov[0].buf = &hdr;
+ msg.iov[0].len = sizeof(hdr);
+ msg.iov[1].buf = data;
+ msg.iov[1].len = data_len;
+
+ /* 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.echo_ans_tx++;
+ ret = xbeeapp_send_msg(addr, &msg, NULL, NULL);
+ callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
+
+ return ret;
+}
+
+/* send an echo message: expect a reply */
+int8_t rc_proto_send_power_probe(uint64_t addr, uint8_t power)
+{
+ struct rc_proto_power_probe hdr;
+ struct xbee_msg msg;
+ uint8_t prio;
+ int8_t ret;
+
+ hdr.type = RC_PROTO_POWER_PROBE;
+ hdr.power_level = power;
+
+ msg.iovlen = 1;
+ msg.iov[0].buf = &hdr;
+ msg.iov[0].len = sizeof(hdr);
+
+ /* set power level */
+ 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.power_probe_tx++;
+ ret = xbeeapp_send_msg(addr, &msg, NULL, NULL);
+ callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
+
+ return ret;
+}
+
+/* convert values from servo_tx.servos into a xbee frame */
+static int8_t servo2buf(uint8_t buf[RC_PROTO_SERVO_LEN],
+ uint8_t seq, uint8_t bypass, uint8_t pow, const uint16_t servos[N_SERVO])
+{
+ uint8_t i = 0;
+
+ buf[i++] = RC_PROTO_SERVO;
+ buf[i++] = ((seq & 0xf) << 4) | (bypass << 3) | (pow & 0x7);
+
+ buf[i++] = servos[0] >> 2;
+ buf[i] = (servos[0] & 0x3) << 6;
+
+ buf[i++] |= servos[1] >> 4;
+ buf[i] = (servos[1] & 0xf) << 4;
+
+ buf[i++] |= servos[2] >> 6;
+ buf[i] = (servos[2] & 0x3f) << 2;
+
+ buf[i++] |= servos[3] >> 8;
+ buf[i++] = servos[3] & 0xff;
+
+ buf[i++] = servos[4] >> 2;
+ buf[i] = (servos[4] & 0x3) << 6;
+
+ buf[i++] |= servos[5] >> 4;
+ buf[i] = (servos[5] & 0xf) << 4;
+
+ return 0;
+}
+
+/* send servos, called periodically with prio = XBEE_PRIO */
+static int8_t rc_proto_send_servos(void)
+{
+ struct rc_proto_servo hdr;
+ struct xbee_msg msg;
+ uint8_t i, updated = 0;
+ uint16_t ms, diff, servo_val;
+ uint8_t frame[RC_PROTO_SERVO_LEN];
+ int8_t ret;
+ uint8_t power;
+
+ /* servo send disabled */
+ if ((rc_proto_flags & RC_PROTO_FLAGS_TX_MASK) == RC_PROTO_FLAGS_TX_OFF)
+ return 0;
+
+ /* if we transmitted servos values recently, nothing to do */
+ ms = get_time_ms();
+ diff = ms - servo_tx.time;
+ if (diff < rc_proto_timers.send_servo_min_ms)
+ return 0;
+
+ /* prepare values to send */
+ if ((rc_proto_flags & RC_PROTO_FLAGS_TX_MASK) ==
+ RC_PROTO_FLAGS_TX_COPY_SPI) {
+
+ /* set bypass to 0 */
+ if (servo_tx.bypass == 1) {
+ servo_tx.bypass = 0;
+ updated = 1;
+ }
+
+ /* copy values from spi */
+ for (i = 0; i < N_SERVO; i++) {
+ servo_val = spi_servo_get(i);
+ if (servo_val != servo_tx.servos[i]) {
+ servo_tx.servos[i] = servo_val;
+ updated = 1;
+ }
+ }
+ }
+ else {
+ /* set bypass to 1 */
+ if (servo_tx.bypass == 0) {
+ servo_tx.bypass = 1;
+ updated = 1;
+ }
+ }
+
+ /* if no value changed and last message is acknowledged, don't transmit
+ * if we already transmitted quite recently */
+ if (updated == 0 && ack == servo_tx.seq &&
+ diff < rc_proto_timers.send_servo_max_ms)
+ return 0;
+
+ /* ok, we need to transmit */
+
+ /* get the new seq value */
+ if (updated == 1) {
+ servo_tx.seq++;
+ servo_tx.seq &= 0x1f;
+ if (servo_tx.seq == ack)
+ servo_tx.seq = (ack - 1) & 0x1f;
+ }
+ /* reset the "updated" flag and save time */
+ servo_tx.time = ms;
+
+ /* set power level */
+ power = get_best_power();
+ xbeeapp_send_atcmd("PL", &power, sizeof(power), NULL, NULL);
+
+ /* create frame and send it */
+ servo2buf(frame, servo_tx.seq, servo_tx.bypass, power, servo_tx.servos);
+ hdr.type = RC_PROTO_SERVO;
+
+ msg.iovlen = 2;
+ msg.iov[0].buf = &hdr;
+ msg.iov[0].len = sizeof(hdr);
+ msg.iov[1].buf = frame;
+ msg.iov[1].len = RC_PROTO_SERVO_LEN;
+
+ stats.servo_tx++;
+ ret = xbeeapp_send_msg(rc_proto_dstaddr, &msg, NULL, NULL);
+ stats.servo_tx++;
+
+ return ret;
+}
+
+/* send a ack message: no answer expected */
+int8_t rc_proto_send_ack(uint64_t addr, uint8_t seq, int8_t power)
+{
+ struct rc_proto_ack hdr;
+ struct xbee_msg msg;
+ uint8_t prio;
+ int8_t ret;
+
+ hdr.type = RC_PROTO_ACK;
+ hdr.seq = seq;
+
+ msg.iovlen = 1;
+ msg.iov[0].buf = &hdr;
+ msg.iov[0].len = sizeof(hdr);
+
+ /* 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.ack_tx++;
+ ret = xbeeapp_send_msg(addr, &msg, NULL, NULL);
+ callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
+
+ return ret;
+}
+
+/* send a hello message: no answer expected */
+int8_t rc_proto_send_stats(uint64_t addr, int8_t power)
+{
+ struct rc_proto_stats hdr;
+ struct xbee_msg msg;
+ uint8_t prio;
+ int8_t ret;
+
+ hdr.type = RC_PROTO_STATS;
+
+ msg.iovlen = 2;
+ msg.iov[0].buf = &hdr;
+ msg.iov[0].len = sizeof(hdr);
+ msg.iov[1].buf = &stats;
+ msg.iov[1].len = sizeof(stats);
+
+ /* 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;
+