X-Git-Url: http://git.droids-corp.org/?p=protos%2Fxbee-avr.git;a=blobdiff_plain;f=rc_proto.c;h=1c1a29fec54d5714af6d5b39b154eb832b5d6af5;hp=8c9166517ffc0a548250b79156604a086e10f599;hb=HEAD;hpb=13e5a26a5242e90e77786b58b5df15d77b7c340c diff --git a/rc_proto.c b/rc_proto.c index 8c91665..1c1a29f 100644 --- a/rc_proto.c +++ b/rc_proto.c @@ -51,13 +51,13 @@ #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 +/* default values */ +struct rc_proto_timers rc_proto_timers = { + .send_servo_min_ms = 50, + .send_servo_max_ms = 300, + .send_power_probe_ms = 500, + .autobypass_ms = 500, +}; /* rc_proto statistics, accessed with sched_prio=XBEE_PRIO */ struct rc_proto_stats_data { @@ -75,6 +75,7 @@ struct rc_proto_stats_data { 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 */ @@ -148,7 +149,7 @@ static void rc_proto_rx_power_probe(int power_level) (void *)(intptr_t)power_level); } -/* called every second */ +/* called every second to decide which power should be used */ static void compute_best_power(void) { int8_t best_power_level = -1; @@ -182,8 +183,13 @@ static void compute_best_power(void) power_level_global = best_power_level; } -static uint8_t get_best_power(void) +/* 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; @@ -239,6 +245,7 @@ int8_t rc_proto_send_echo_req(uint64_t addr, void *data, uint8_t data_len, hdr.type = RC_PROTO_ECHO_REQ; hdr.power = power; + hdr.timestamp = get_time_ms(); hdr.datalen = data_len; msg.iovlen = 2; @@ -262,7 +269,7 @@ int8_t rc_proto_send_echo_req(uint64_t addr, void *data, uint8_t data_len, /* 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) + int8_t power, uint16_t timestamp) { struct rc_proto_echo_ans hdr; struct xbee_msg msg; @@ -271,6 +278,7 @@ int8_t rc_proto_send_echo_ans(uint64_t addr, void *data, uint8_t data_len, hdr.type = RC_PROTO_ECHO_ANS; hdr.datalen = data_len; + hdr.timestamp = timestamp; msg.iovlen = 2; msg.iov[0].buf = &hdr; @@ -366,7 +374,7 @@ static int8_t rc_proto_send_servos(void) /* 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) + if (diff < rc_proto_timers.send_servo_min_ms) return 0; /* prepare values to send */ @@ -399,7 +407,7 @@ static int8_t rc_proto_send_servos(void) /* 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) + diff < rc_proto_timers.send_servo_max_ms) return 0; /* ok, we need to transmit */ @@ -605,7 +613,8 @@ int rc_proto_rx(struct xbee_recv_hdr *recvframe, unsigned len) stats.echo_req_rx++; if (rc_proto_send_echo_ans(ntohll(recvframe->srcaddr), - rce->data, rce->datalen, power) < 0) + rce->data, rce->datalen, power, + rce->timestamp) < 0) return -1; return 0; @@ -614,9 +623,12 @@ int rc_proto_rx(struct xbee_recv_hdr *recvframe, unsigned len) 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; } @@ -706,7 +718,7 @@ static void rc_proto_cb(struct callout_mgr *cm, struct callout *tim, void *arg) /* 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) { + if (diff > rc_proto_timers.send_power_probe_ms) { pow_probe++; if (pow_probe > 4) pow_probe = 0; @@ -718,15 +730,17 @@ static void rc_proto_cb(struct callout_mgr *cm, struct callout *tim, void *arg) /* 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) + if (diff > rc_proto_timers.autobypass_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; + 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 */ @@ -758,6 +772,10 @@ void rc_proto_dump_stats(void) 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); @@ -774,6 +792,10 @@ void rc_proto_dump_stats(void) 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)