#include <aversive.h>
#include <aversive/queue.h>
+#include <aversive/endian.h>
#include <stdint.h>
#include "callout.h"
#include "rc_proto.h"
#include "xbee_user.h"
+#include "spi_servo.h"
#include "main.h"
-/* */
+#define N_SERVO 6
+#define SERVO_NBITS 10
+
+#define RX_DB_THRESHOLD 65 /* mean -65 dB */
+
+/* XXX make it configurable */
+/* min time between 2 servo_send */
+#define SEND_SERVO_MIN_TIME_MS 50
+/* max time between 2 servo_send */
+#define SEND_SERVO_MAX_TIME_MS 300
+/* time before switching into bypass mode when no servo command received */
+#define AUTO_BYPASS_TIME_MS 500
+
+/* rc_proto statistics, accessed with sched_prio=XBEE_PRIO */
+struct rc_proto_stats_data {
+ uint32_t hello_rx;
+ uint32_t hello_tx;
+ uint32_t echo_req_rx;
+ uint32_t echo_req_tx;
+ uint32_t echo_ans_rx;
+ uint32_t echo_ans_tx;
+ uint32_t power_probe_rx;
+ uint32_t power_probe_tx;
+ uint32_t ack_rx;
+ uint32_t ack_tx;
+ uint32_t servo_rx;
+ uint32_t servo_tx;
+ uint32_t stats_rx;
+ uint32_t stats_tx;
+ uint32_t echo_ans_latency_sum;
+};
+static struct rc_proto_stats_data stats; /* local stats */
+static struct rc_proto_stats_data peer_stats; /* peer stats */
+
+/* store last received power probes */
struct rc_proto_power_levels {
uint8_t ttl;
uint16_t power_db;
};
static struct rc_proto_power_levels power_levels[MAX_POWER_LEVEL];
-/* update power level when we receive the answer from DB */
-static int update_power_level(void *frame, unsigned len, void *arg)
+/* address of the peer */
+static uint64_t rc_proto_dstaddr = 0xFFFF; /* broadcast by default */
+
+/* state sent to the xbee peer (used on RC) */
+struct servo_tx {
+ uint16_t servos[N_SERVO];
+ uint8_t bypass; /* ask the wing to bypass servos = use legacy controller */
+ uint8_t seq; /* from 0 to 15, 4 bits */
+ uint16_t time; /* time of last xmit */
+};
+static struct servo_tx servo_tx;
+
+/* state received from the xbee peer (used on WING) */
+struct servo_rx {
+ uint16_t servos[N_SERVO];
+ uint16_t time; /* time of last xmit */
+};
+static struct servo_rx servo_rx;
+
+/* the received seq value (acknowledged by the wing, received on the rc) */
+static uint8_t ack;
+
+/* define tx mode (disabled, send from spi, bypass), rx mode (auto-bypass),
+ ... */
+static uint8_t rc_proto_flags;
+
+/* callout managing rc_proto (ex: sending of servos periodically) */
+static struct callout rc_proto_timer;
+
+/* a negative value (-1 or -4) means we don't know the best level, but it stores
+ * the previous PL value (0 or 4) so we can alternate. */
+int8_t power_level_global = -1;
+
+/* update power level when we receive the answer from DB. The request is sent by
+ * rc_proto_rx_power_probe(). */
+static int8_t update_power_level(int8_t retcode, void *frame, unsigned len,
+ void *arg)
{
struct xbee_atresp_hdr *atresp = (struct xbee_atresp_hdr *)frame;
int level = (intptr_t)arg;
uint8_t db;
- /* XXX check if this test is correct */
- if (len < sizeof(struct xbee_atresp_hdr) + sizeof(uint8_t)) {
- /* XXX stats */
+ /* nothing more to do, error is already logged by xbee_user */
+ if (retcode < 0)
+ return retcode;
+
+ if (len < sizeof(struct xbee_atresp_hdr) + sizeof(uint8_t))
return -1;
- }
db = atresp->data[0];
power_levels[level].power_db = db;
- power_levels[level].ttl = 2;
+ power_levels[level].ttl = 10; /* valid during 10 seconds */
return 0;
}
/* when we receive a power probe, ask the DB value to the xbee */
-void rc_proto_rx_power_probe(int power_level)
+static void rc_proto_rx_power_probe(int power_level)
{
- (void)power_level;
- xbeeapp_send_atcmd("DB", NULL, 0, 0, update_power_level, NULL);
+ xbeeapp_send_atcmd("DB", NULL, 0, update_power_level,
+ (void *)(intptr_t)power_level);
}
-/* send a hello message */
-// XXX iovec for xbee ?
-int8_t rc_proto_send_hello(uint64_t addr, void *data, uint8_t data_len)
+/* called every second */
+static void compute_best_power(void)
{
- struct {
- struct rc_proto_echo_req hdr;
- char buf[XBEE_MAX_FRAME_LEN - sizeof(struct rc_proto_echo_req)];
- } frame;
+ int8_t best_power_level = -1;
+ int8_t i;
- if (data_len > sizeof(frame.buf))
- return -1;
+ /* 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;
- frame.hdr.type = RC_PROTO_HELLO;
- frame.hdr.datalen = data_len;
- memcpy(frame.buf, data, data_len);
- return xbeeapp_send_msg(addr, &frame,
- data_len + sizeof(struct rc_proto_echo_req), 1);
+ 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;
}
+static uint8_t get_best_power(void)
+{
+ /* 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;
+}
-#if 0
-#define N_SERVO 6
-#define SERVO_NBITS 10
-uint16_t servos[N_SERVO] = { 0x123, 0x234, 0x321, 0x123, 0x234, 0x321 };
+/* 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;
+}
-int8_t servo2buf(uint8_t *buf, uint8_t len, uint8_t servo_mask,
- uint8_t pow, uint8_t seq)
+/* 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)
{
- uint8_t i = 0, num;
- uint8_t remain_bits;
- uint8_t servo_bits = 0;
- uint16_t servo_val;
- uint8_t tmp;
+ 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;
+}
- if (len < 2)
- return -1;
+/* 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)
+{
+ 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;
+
+ 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;
+}
- memset(buf, 0, len);
- buf[i++] = servo_mask;
- buf[i++] = ((seq & 0x1f) << 3) | (pow & 0x7);
- remain_bits = 8;
+/* 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;
- for (num = 0; num < N_SERVO; num++) {
- if (!(servo_mask & (1 << num)))
- continue;
- servo_val = servos[num];
- servo_bits = SERVO_NBITS;
+ hdr.type = RC_PROTO_POWER_PROBE;
+ hdr.power_level = power;
- tmp = (servo_val >> (servo_bits - remain_bits));
- tmp &= ((1 << remain_bits) - 1);
- if (i >= len)
- return -1;
- buf[i++] |= tmp;
+ msg.iovlen = 1;
+ msg.iov[0].buf = &hdr;
+ msg.iov[0].len = sizeof(hdr);
- servo_bits = 10 - remain_bits;
- tmp = servo_val & ((1 << servo_bits) - 1);
- tmp <<= (8 - servo_bits);
- if (i >= len)
- return -1;
- buf[i] = tmp;
+ /* 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;
- if (servo_bits == 8) {
- i++;
- remain_bits = 8;
+ 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 < SEND_SERVO_MIN_TIME_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;
}
- else
- remain_bits = 8 - servo_bits;
}
- if (remain_bits != 8)
- i++;
+ /* 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 < SEND_SERVO_MAX_TIME_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;
+}
- return i;
+uint8_t rc_proto_get_mode(void)
+{
+ return rc_proto_flags;
}
-int8_t buf2servo(uint8_t *buf, uint8_t len)
+/* convert a receved servo frame into servo values */
+static int8_t buf2servo(uint16_t servos[N_SERVO], const uint8_t *buf)
{
- uint8_t mask, count = 0;
- uint8_t num = 0;
- uint8_t pow, seq;
uint16_t val;
- if (len < 2)
- return -1;
+ 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;
- mask = buf[0];
- if (mask > 0x3f)
+ /* convert it in a table of servo values */
+ if (bypass == 0 && buf2servo(servo_rx.servos, rcs->data) < 0)
return -1;
- pow = buf[1] & 0x07;
- seq = buf[1] >> 5;
- for (num = 0; num < N_SERVO; num++) {
- if ((1<<num) & mask)
- count++;
- }
- switch (count) {
- case 1: if (len != 4) return -1; break;
- case 2: if (len != 5) return -1; break;
- case 3: if (len != 6) return -1; break;
- case 4: if (len != 7) return -1; break;
- case 5: if (len != 9) return -1; break;
- case 6: if (len != 10) return -1; break;
- default: 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;
- for (num = 0; ((1<<num) & mask) == 0; num++) {
- if (num >= N_SERVO)
+ /* 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;
- }
+ }
- val = buf[2];
- val <<= 2;
- val |= (buf[3] >> 6);
+ 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) < 0)
+ return -1;
- for (num++; ((1<<num) & mask) == 0; num++) {
- if (num >= N_SERVO)
return 0;
- }
+ }
- val = buf[3] & 0x3f;
- val <<= 4;
- val |= (buf[4] >> 4);
+ case RC_PROTO_ECHO_ANS: {
+ struct rc_proto_echo_ans *rce =
+ (struct rc_proto_echo_ans *) recvframe->data;
+ uint16_t diff;
- for (num++; ((1<<num) & mask) == 0; num++) {
- if (num >= N_SERVO)
+ 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;
- }
+ }
- val = buf[4] & 0xf;
- val <<= 6;
- val |= (buf[5] >> 2);
+ /* 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);
- for (num++; ((1<<num) & mask) == 0; num++) {
- if (num >= N_SERVO)
return 0;
- }
+ }
- val = buf[5] & 0x3;
- val <<= 8;
- val |= (buf[6]);
+ /* received by the radio controller */
+ case RC_PROTO_ACK: {
+ struct rc_proto_ack *rca =
+ (struct rc_proto_ack *) recvframe->data;
- for (num++; ((1<<num) & mask) == 0; num++) {
- if (num >= N_SERVO)
+ NOTICE(E_USER_XBEE, "recv ack, ack=%d", rca->seq);
+ stats.ack_rx++;
return 0;
- }
+ }
- val = buf[7];
- val <<= 2;
- val |= (buf[8] >> 6);
+ /* 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);
+ }
- for (num++; ((1<<num) & mask) == 0; num++) {
- if (num >= N_SERVO)
+ /* 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;
- }
+ }
- val = buf[8];
- val <<= 4;
- val |= (buf[9] >> 4);
+ default:
+ return -1;
+ }
+ /* not reached */
return 0;
}
-#endif
+
+/* 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 > AUTO_BYPASS_TIME_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 > AUTO_BYPASS_TIME_MS)
+ spi_servo_set_bypass(1);
+ }
+
+ /* send stats to peer every second */
+ 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 (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);
+}
git.droids-corp.org / protos / xbee-avr.git / blobdiff