From: Olivier Matz Date: Thu, 3 Apr 2014 18:49:57 +0000 (+0200) Subject: add a complete rc_proto stack (not tested) X-Git-Url: http://git.droids-corp.org/?a=commitdiff_plain;h=f820d95b15d8bbb7c289acf92f31848d218825a7;p=protos%2Fxbee-avr.git add a complete rc_proto stack (not tested) --- diff --git a/commands.c b/commands.c index b64dfb1..f525e5f 100644 --- a/commands.c +++ b/commands.c @@ -667,7 +667,7 @@ static void cmd_send_hello_parsed(void *parsed_result, void *use_neigh) if (diff < res->period) continue; - rc_proto_send_hello(addr, res->data, strlen(res->data)); + rc_proto_send_hello(addr, res->data, strlen(res->data), -1); next += res->period; res->count--; } diff --git a/main.c b/main.c index 11fd442..a0839d2 100644 --- a/main.c +++ b/main.c @@ -89,7 +89,7 @@ void bootloader(void) } /* return time in milliseconds on unsigned 16 bits */ -static uint16_t get_time_ms(void) +uint16_t get_time_ms(void) { uint16_t ms; uint8_t flags; @@ -182,6 +182,8 @@ int main(void) xbeeapp_init(); + rc_proto_init(); + sei(); eeprom_load_config(); diff --git a/main.h b/main.h index d83c5fa..0f87a4f 100644 --- a/main.h +++ b/main.h @@ -88,3 +88,4 @@ extern struct xbeeboard xbeeboard; extern volatile uint32_t global_ms; void bootloader(void); +uint16_t get_time_ms(void); diff --git a/notes.txt b/notes.txt index aac6707..4096dfa 100644 --- a/notes.txt +++ b/notes.txt @@ -216,4 +216,74 @@ Timeout for xbee commands - log (hexdump) - the frame must be checked in the cb, we can provide helpers +Tests to do +=========== +:: + + Radio Controller ))) xbee ((( WING + / + / + PC with command line + (through serial) + +Test 1: send data, unidirectional +--------------------------------- + +Periodically send data from RC to WING (every 20 to 100 ms). The WING sends its +statistics every second. + +Variables: + +- period: from 20ms to 200ms +- packet size: from 1 byte to 20 bytes +- total number of packets: 1K for short tests, 100K for robustness + +If the test is long, take care about maximum duty cycles (10%). + +Output: + +- local and peer statistics for each test (packet size, pps, number of packets) +- at the end we will know what is the highest speed we can support for a given + packet size, and how reliable is the xbee + +Test 2: send data, bidirectional +-------------------------------- + +Same than above, except that the WING replies to each received packet with a +packet of same size. + +Same kind of output. + +Test 3: test wing control on ground +----------------------------------- + +On the ground, try to control the wing with the RC board. We also use the 2.4 +Ghz controller as a fallback (bypass mode). + +Check that the bypass mode is enabled when we ask it (for instance on the +channnel 5 switch) or when we switch off the RC board. + +Test 4: embed in WING and send dummy servo commands +--------------------------------------------------- + +Test in real conditions: the WING board is embeded during a flight. The RC board +sends dummy servo values at a specified rate (choosen from results of test 1 and +2). The WING sends statistics and "power probes" allowing the RC board to choose +its tx power level. + +Check how the wing distance impacts: +- the tx power level of the RC board +- the statistics +- the RX DB + +Maybe check with and without the 433Mhz beacon. + +Output: + +After this test, we should be confident that xbee is useable in real conditions. + +Test 5: control the wing with the RC board +------------------------------------------ + +Same than test 3, but in real flight conditions. diff --git a/rc_proto.c b/rc_proto.c index 4ced123..cc6cce4 100644 --- a/rc_proto.c +++ b/rc_proto.c @@ -29,6 +29,7 @@ #include #include +#include #include @@ -42,16 +43,84 @@ #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 */ +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; +}; +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 */ +/* 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) { @@ -63,30 +132,79 @@ static int8_t update_power_level(int8_t retcode, void *frame, unsigned len, if (retcode < 0) return retcode; - /* XXX check if this test is correct */ - if (len < sizeof(struct xbee_atresp_hdr) + sizeof(uint8_t)) { - /* XXX stats */ + 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, update_power_level, NULL); + xbeeapp_send_atcmd("DB", NULL, 0, update_power_level, + (void *)(intptr_t)power_level); } -/* send a hello message */ -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 rc_proto_echo_req hdr; + 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; +} + +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; +} + +/* 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; @@ -97,153 +215,364 @@ int8_t rc_proto_send_hello(uint64_t addr, void *data, uint8_t data_len) msg.iov[1].buf = data; msg.iov[1].len = data_len; - return xbeeapp_send_msg(addr, &msg, NULL, NULL); + /* 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; -#if 0 -#define N_SERVO 6 -#define SERVO_NBITS 10 -uint16_t servos[N_SERVO] = { 0x123, 0x234, 0x321, 0x123, 0x234, 0x321 }; + hdr.type = RC_PROTO_ECHO_REQ; + hdr.power = power; + hdr.datalen = data_len; -int8_t servo2buf(uint8_t *buf, uint8_t len, uint8_t servo_mask, - uint8_t pow, uint8_t seq) + 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) { - 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_ans hdr; + struct xbee_msg msg; + uint8_t prio; + int8_t ret; - if (len < 2) - return -1; + 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; - memset(buf, 0, len); - buf[i++] = servo_mask; - buf[i++] = ((seq & 0x1f) << 3) | (pow & 0x7); - remain_bits = 8; + /* set power level */ + if (power != -1) + xbeeapp_send_atcmd("PL", &power, sizeof(power), NULL, NULL); - for (num = 0; num < N_SERVO; num++) { - if (!(servo_mask & (1 << num))) - continue; - servo_val = servos[num]; - servo_bits = SERVO_NBITS; + /* 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); - tmp = (servo_val >> (servo_bits - remain_bits)); - tmp &= ((1 << remain_bits) - 1); - if (i >= len) - return -1; - buf[i++] |= tmp; + return ret; +} - servo_bits = 10 - remain_bits; - tmp = servo_val & ((1 << servo_bits) - 1); - tmp <<= (8 - servo_bits); - if (i >= len) - return -1; - buf[i] = tmp; +/* 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; - if (servo_bits == 8) { - i++; - remain_bits = 8; - } - else - remain_bits = 8 - servo_bits; - } + 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); - if (remain_bits != 8) - i++; + /* 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 i; + return ret; } -int8_t buf2servo(uint8_t *buf, uint8_t len) +/* 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 mask, count = 0; - uint8_t num = 0; - uint8_t pow, seq; - uint16_t val; + uint8_t i = 0; - if (len < 2) - return -1; + buf[i++] = RC_PROTO_SERVO; + buf[i++] = ((seq & 0xf) << 4) | (bypass << 3) | (pow & 0x7); - mask = buf[0]; - if (mask > 0x3f) - return -1; - pow = buf[1] & 0x07; - seq = buf[1] >> 5; + 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 < 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; + } - for (num = 0; num < N_SERVO; num++) { - if ((1<= N_SERVO) - return 0; - } + /* 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; - val = buf[2]; - val <<= 2; - val |= (buf[3] >> 6); + /* ok, we need to transmit */ - for (num++; ((1<= N_SERVO) - return 0; + /* 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; - val = buf[3] & 0x3f; - val <<= 4; - val |= (buf[4] >> 4); + /* set power level */ + power = get_best_power(); + xbeeapp_send_atcmd("PL", &power, sizeof(power), NULL, NULL); - for (num++; ((1<= N_SERVO) - return 0; - } + /* create frame and send it */ + servo2buf(frame, servo_tx.seq, servo_tx.bypass, power, servo_tx.servos); + hdr.type = RC_PROTO_SERVO; - val = buf[4] & 0xf; - val <<= 6; - val |= (buf[5] >> 2); + 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; - for (num++; ((1<= N_SERVO) - return 0; - } + stats.servo_tx++; + ret = xbeeapp_send_msg(rc_proto_dstaddr, &msg, NULL, NULL); + stats.servo_tx++; - val = buf[5] & 0x3; - val <<= 8; - val |= (buf[6]); + return ret; +} - for (num++; ((1<= N_SERVO) - return 0; - } +/* 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; - val = buf[7]; + 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[8] >> 6); + val |= (buf[2] >> 6); + servos[0] = val; - for (num++; ((1<= N_SERVO) - return 0; - } + 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[8]; + 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[9] >> 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; } -#endif +/* receive a rc_proto message */ int rc_proto_rx(struct xbee_recv_hdr *recvframe, unsigned len) { unsigned int datalen; @@ -256,48 +585,236 @@ int rc_proto_rx(struct xbee_recv_hdr *recvframe, unsigned len) if (datalen < sizeof(struct rc_proto_hdr)) return -1; + /* other command types */ switch (rch->type) { -#if 0 - case RC_PROTO_TYPE_CHANNEL: { - struct rc_proto_channel *rcc = - (struct rc_proto_channel *) recvframe->data; - int16_t val; - if (datalen != sizeof(struct rc_proto_channel)) + 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) < 0) return -1; - val = ntohs(rcc->axis[0]); - val >>= 6; - val += 512; - spi_servo_set(0, val); - break; + + return 0; } -#endif + + case RC_PROTO_ECHO_ANS: { + struct rc_proto_echo_ans *rce = + (struct rc_proto_echo_ans *) recvframe->data; + + NOTICE(E_USER_XBEE, "recv echo_ans len=%d", rce->datalen); + stats.echo_ans_rx++; + 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; - //rc_proto_rx_range(rcpb->power_level); + stats.power_probe_rx++; + /* ask the DB value to the xbee module */ + rc_proto_rx_power_probe(rcpb->power_level); - break; + return 0; } - case RC_PROTO_HELLO: { - struct rc_proto_hello *rch = - (struct rc_proto_hello *) recvframe->data; + /* 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 hello len=%d", - rch->datalen); - /* XXX stats */ - break; + 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 > 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); + + 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); +} + +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); +} diff --git a/rc_proto.h b/rc_proto.h index 868f921..2a2a666 100644 --- a/rc_proto.h +++ b/rc_proto.h @@ -6,8 +6,9 @@ struct rc_proto_hdr { uint8_t type; } __attribute__((packed)); + /* send a hello message, no answer is expected from the peer */ -#define RC_PROTO_HELLO 0 +#define RC_PROTO_HELLO 0x00 struct rc_proto_hello { uint8_t type; uint8_t datalen; /* len of data excluding header */ @@ -15,48 +16,119 @@ struct rc_proto_hello { } __attribute__((packed)); /* send an echo request, expect an echo reply from the peer */ -#define RC_PROTO_ECHO_REQ 1 +#define RC_PROTO_ECHO_REQ 0x01 struct rc_proto_echo_req { uint8_t type; + int8_t power; uint8_t datalen; /* len of data excluding header */ uint8_t data[]; } __attribute__((packed)); /* reply to an echo request */ -#define RC_PROTO_ECHO_ANS 2 +#define RC_PROTO_ECHO_ANS 0x02 struct rc_proto_echo_ans { uint8_t type; uint8_t datalen; /* len of data excluding header */ uint8_t data[]; } __attribute__((packed)); -/* send a power level probe to the peer */ -#define RC_PROTO_POWER_PROBE 3 +/* send a power level probe to the peer: no answer is expected, but the peer + * will know that a packet with this power-level is received. It can also ask + * the RSSI of this packet. */ +#define RC_PROTO_POWER_PROBE 0x03 struct rc_proto_power_probe { uint8_t type; uint8_t power_level; } __attribute__((packed)); -/* send a servo command */ -#define RC_PROTO_SERVO 4 -struct rc_proto_servo { - uint8_t type; - uint8_t mask; - uint8_t seq_and_pow; /* bitfield: pow are the 3 lsb, seq the 5 msb */ -}; - /* acknowledge a servo command */ -#define RC_PROTO_ACK 5 +#define RC_PROTO_ACK 0x04 struct rc_proto_ack { uint8_t type; uint8_t seq; } __attribute__((packed)); +/* + * If type < 0x3f, it's a servo command. The size of the message is critical + * because it's send very often. The "type" field contains the bitfield of + * servos present in the body of the message. A sequence number (5bits) and the + * power level (3 bits) are also sent. The servos are listed as 10bits fields, + * without padding. + * + * Example: we send servo 0 (=0x123) and servo 3 (=0x234). Power = 2. + * command type (RC_PROTO_SERVO) + * power_level = 0 (3 bits, LSB) \ + * bypass = 0 (1 bit) > one byte + * seq = 1 (4 bits, MSB) / + * servo_val[0]=0x123 + * servo_val[1]=0x234 + * servo_val[2]=0x321 + * servo_val[3]=0x123 + * servo_val[4]=0x234 + * servo_val[5]=0x321 + * + * -> 0x05 0x10 0x48 0xe3 0x4c 0x85 0x23 0x8d 0x32 0x10 + * type psb |ser0 ser1 ser2 ser3 |ser4 ser5 + */ +#define RC_PROTO_SERVO 0x05 +#define RC_PROTO_SERVO_LEN 10 /* len of frame */ +struct rc_proto_servo { + uint8_t type; + uint8_t data[]; +}; + +/* send stats */ +#define RC_PROTO_STATS 0x06 +struct rc_proto_stats { + uint8_t type; + uint8_t stats[]; /* format is struct rc_proto_stats_data */ +} __attribute__((packed)); /* send a Hello message to a peer */ -int8_t rc_proto_send_hello(uint64_t addr, void *data, uint8_t data_len); +int8_t rc_proto_send_hello(uint64_t addr, void *data, uint8_t data_len, + int8_t power); + +int8_t rc_proto_send_echo_req(uint64_t addr, void *data, uint8_t data_len, + int8_t power); /* reception of a xbee message */ int rc_proto_rx(struct xbee_recv_hdr *recvframe, unsigned len); +/* dmp statistics related to rc_proto */ +void rc_proto_dump_stats(void); + +/* set the peer xbee address */ +void rc_proto_set_dstaddr(uint64_t addr); + +/* get the peer xbee address */ +uint64_t rc_proto_get_dstaddr(void); + +void rc_proto_dump_servos(void); + +/* tx mode */ +#define RC_PROTO_FLAGS_TX_OFF 0x00 +#define RC_PROTO_FLAGS_TX_BYPASS 0x01 +#define RC_PROTO_FLAGS_TX_COPY_SPI 0x02 +#define RC_PROTO_FLAGS_TX_RESERVED 0x03 +#define RC_PROTO_FLAGS_TX_MASK 0x03 + +/* if set, copy received servo values to SPI */ +#define RC_PROTO_FLAGS_RX_COPY_SPI 0x04 + +/* if set, switch to bypass when no servo is received during some time */ +#define RC_PROTO_FLAGS_RX_AUTOBYPASS 0x08 + +/* if set, send stats periodically to the peer (1 sec) */ +#define RC_PROTO_FLAGS_TX_STATS 0x10 + +/* if set, send power probe periodically to the peer (500 ms) */ +#define RC_PROTO_FLAGS_TX_POW_PROBE 0x20 + +void rc_proto_set_mode(uint8_t flags); + +uint8_t rc_proto_get_mode(void); + +/* initialize rc_proto module */ +void rc_proto_init(void); + #endif diff --git a/xbee_user.c b/xbee_user.c index 859d203..9d3da99 100644 --- a/xbee_user.c +++ b/xbee_user.c @@ -237,7 +237,9 @@ static int parse_atcmd(struct xbee_ctx *ctx, struct xbee_atresp_hdr *frame, } -/* main rx entry point for application */ +/* Main xbee rx entry point for application. It decodes the xbee frame type and + * dispatch to the application layer. Then "len" argument does not include the + * xbee_hdr structure (delimiter, len, type, id) and checksum. */ int8_t xbeeapp_rx(struct xbee_dev *dev, int channel, int type, void *frame, unsigned len, void *opaque) {