From f06d6833b5fc78c264b403ab4fa53fec95f34dab Mon Sep 17 00:00:00 2001 From: Olivier Matz Date: Thu, 24 Jul 2014 20:14:33 +0200 Subject: [PATCH] support imu and gps over xbee (not tested) --- mainboard/commands.c | 39 +++++++- mainboard/rc_proto.c | 210 +++++++++++++++++++++++++++++++++++-------- mainboard/rc_proto.h | 63 ++++++++++--- 3 files changed, 256 insertions(+), 56 deletions(-) diff --git a/mainboard/commands.c b/mainboard/commands.c index 1837fda..d0ff2a1 100644 --- a/mainboard/commands.c +++ b/mainboard/commands.c @@ -1622,6 +1622,8 @@ struct cmd_rc_proto_timers_result { uint16_t servo_min; uint16_t servo_max; uint16_t power_probe; + uint16_t gps_pos; + uint16_t imu_pos; uint16_t autobypass; }; @@ -1634,14 +1636,18 @@ static void cmd_rc_proto_timers_parsed(void *parsed_result, void *data) rc_proto_timers.send_servo_min_ms = res->servo_min; rc_proto_timers.send_servo_max_ms = res->servo_max; rc_proto_timers.send_power_probe_ms = res->power_probe; + rc_proto_timers.send_gps_pos_ms = res->gps_pos; + rc_proto_timers.send_imu_pos_ms = res->imu_pos; rc_proto_timers.autobypass_ms = res->autobypass; } printf_P(PSTR("rc_proto_timers: min=%d, max=%d, " - "power_probe=%d autobypass=%d\n"), + "power_probe=%d gps_pos=%d imu_pos=%d autobypass=%d\n"), rc_proto_timers.send_servo_min_ms, rc_proto_timers.send_servo_max_ms, rc_proto_timers.send_power_probe_ms, + rc_proto_timers.send_gps_pos_ms, + rc_proto_timers.send_imu_pos_ms, rc_proto_timers.autobypass_ms); } @@ -1662,12 +1668,18 @@ const parse_token_num_t PROGMEM cmd_rc_proto_timers_servo_max = const parse_token_num_t PROGMEM cmd_rc_proto_timers_power_probe = TOKEN_NUM_INITIALIZER(struct cmd_rc_proto_timers_result, power_probe, UINT16); +const parse_token_num_t PROGMEM cmd_rc_proto_timers_gps_pos = + TOKEN_NUM_INITIALIZER(struct cmd_rc_proto_timers_result, gps_pos, + UINT16); +const parse_token_num_t PROGMEM cmd_rc_proto_timers_imu_pos = + TOKEN_NUM_INITIALIZER(struct cmd_rc_proto_timers_result, imu_pos, + UINT16); const parse_token_num_t PROGMEM cmd_rc_proto_timers_autobypass = TOKEN_NUM_INITIALIZER(struct cmd_rc_proto_timers_result, autobypass, UINT16); const char PROGMEM help_rc_proto_timers[] = "set rc_proto_timers (servo_min, " - "servo_max, pow_probe, autobypass)"; + "servo_max, pow_probe, gps_pos, imu_pos, autobypass)"; const parse_inst_t PROGMEM cmd_rc_proto_timers = { .f = cmd_rc_proto_timers_parsed, /* function to call */ .data = NULL, /* 2nd arg of func */ @@ -1678,6 +1690,8 @@ const parse_inst_t PROGMEM cmd_rc_proto_timers = { (PGM_P)&cmd_rc_proto_timers_servo_min, (PGM_P)&cmd_rc_proto_timers_servo_max, (PGM_P)&cmd_rc_proto_timers_power_probe, + (PGM_P)&cmd_rc_proto_timers_gps_pos, + (PGM_P)&cmd_rc_proto_timers_imu_pos, (PGM_P)&cmd_rc_proto_timers_autobypass, NULL, }, @@ -1713,7 +1727,7 @@ static void cmd_rc_proto_mode_parsed(void *parsed_result, void *data) { struct cmd_rc_proto_mode_result *res = parsed_result; (void)data; - uint8_t flags; + uint16_t flags; uint8_t on = 0; flags = rc_proto_get_mode(); @@ -1744,6 +1758,18 @@ static void cmd_rc_proto_mode_parsed(void *parsed_result, void *data) else flags &= ~RC_PROTO_FLAGS_TX_POW_PROBE; } + else if (!strcmp_P(res->cmd, PSTR("tx_gps_pos"))) { + if (on == 1) + flags |= RC_PROTO_FLAGS_TX_GPS_POS; + else + flags &= ~RC_PROTO_FLAGS_TX_GPS_POS; + } + else if (!strcmp_P(res->cmd, PSTR("tx_imu_pos"))) { + if (on == 1) + flags |= RC_PROTO_FLAGS_TX_IMU_POS; + else + flags &= ~RC_PROTO_FLAGS_TX_IMU_POS; + } else if (!strcmp_P(res->cmd, PSTR("compute_best_pow"))) { if (on == 1) flags |= RC_PROTO_FLAGS_COMPUTE_BEST_POW; @@ -1774,6 +1800,10 @@ static void cmd_rc_proto_mode_parsed(void *parsed_result, void *data) (flags & RC_PROTO_FLAGS_TX_STATS) ? "on" : "off"); printf_P(PSTR("rc_proto_mode tx_power_probe %s\n"), (flags & RC_PROTO_FLAGS_TX_POW_PROBE) ? "on" : "off"); + printf_P(PSTR("rc_proto_mode tx_gps_pos %s\n"), + (flags & RC_PROTO_FLAGS_TX_GPS_POS) ? "on" : "off"); + printf_P(PSTR("rc_proto_mode tx_imu_pos %s\n"), + (flags & RC_PROTO_FLAGS_TX_IMU_POS) ? "on" : "off"); printf_P(PSTR("rc_proto_mode compute_best_pow %s\n"), (flags & RC_PROTO_FLAGS_COMPUTE_BEST_POW) ? "on" : "off"); } @@ -1784,7 +1814,8 @@ const parse_token_string_t PROGMEM cmd_rc_proto_mode_arg0 = str_rc_proto_mode_arg0); const char PROGMEM str_rc_proto_mode_cmd[] = - "rx_copy_spi#rx_autobypass#tx_stats#tx_power_probe#compute_best_pow"; + "rx_copy_spi#rx_autobypass#tx_stats#tx_power_probe#" + "tx_gps_pos#tx_imu_pos#compute_best_pow"; const parse_token_string_t PROGMEM cmd_rc_proto_mode_cmd = TOKEN_STRING_INITIALIZER(struct cmd_rc_proto_mode_result, cmd, str_rc_proto_mode_cmd); diff --git a/mainboard/rc_proto.c b/mainboard/rc_proto.c index 1c1a29f..0e1b9cc 100644 --- a/mainboard/rc_proto.c +++ b/mainboard/rc_proto.c @@ -44,6 +44,8 @@ #include "rc_proto.h" #include "xbee_user.h" #include "spi_servo.h" +#include "../common/i2c_commands.h" +#include "i2c_protocol.h" #include "main.h" #define N_SERVO 6 @@ -56,6 +58,8 @@ struct rc_proto_timers rc_proto_timers = { .send_servo_min_ms = 50, .send_servo_max_ms = 300, .send_power_probe_ms = 500, + .send_gps_pos_ms = 500, + .send_imu_pos_ms = 100, .autobypass_ms = 500, }; @@ -75,6 +79,10 @@ struct rc_proto_stats_data { uint32_t servo_tx; uint32_t stats_rx; uint32_t stats_tx; + uint32_t gps_pos_rx; + uint32_t gps_pos_tx; + uint32_t imu_pos_rx; + uint32_t imu_pos_tx; uint32_t echo_ans_latency_sum; }; static struct rc_proto_stats_data stats; /* local stats */ @@ -111,7 +119,7 @@ static uint8_t ack; /* define tx mode (disabled, send from spi, bypass), rx mode (auto-bypass), ... */ -static uint8_t rc_proto_flags; +static uint16_t rc_proto_flags; /* callout managing rc_proto (ex: sending of servos periodically) */ static struct callout rc_proto_timer; @@ -500,12 +508,84 @@ int8_t rc_proto_send_stats(uint64_t addr, int8_t power) return ret; } -void rc_proto_set_mode(uint8_t flags) +/* send a gps_pos message: no answer expected */ +int8_t rc_proto_send_gps_pos(uint64_t addr, int8_t power) +{ + struct rc_proto_gps_pos gps_msg; + struct xbee_msg msg; + uint8_t prio; + int8_t ret; + uint8_t irq_flags; + + gps_msg.type = RC_PROTO_GPS_POS; + + IRQ_LOCK(irq_flags); + gps_msg.valid = !!(imuboard_status.flags & I2C_IMUBOARD_STATUS_GPS_OK); + gps_msg.latitude = imuboard_status.latitude; + gps_msg.longitude = imuboard_status.longitude; + gps_msg.altitude = imuboard_status.altitude; + IRQ_UNLOCK(irq_flags); + + msg.iovlen = 1; + msg.iov[0].buf = &gps_msg; + msg.iov[0].len = sizeof(gps_msg); + + /* set power level */ + if (power != -1) + xbeeapp_send_atcmd("PL", &power, sizeof(power), NULL, NULL); + + /* we also need to lock callout to increment stats */ + prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO); + stats.gps_pos_tx++; + ret = xbeeapp_send_msg(addr, &msg, NULL, NULL); + callout_mgr_restore_prio(&xbeeboard.intr_cm, prio); + + return ret; +} + +/* send a imu_pos message: no answer expected */ +int8_t rc_proto_send_imu_pos(uint64_t addr, int8_t power) +{ + struct rc_proto_imu_pos imu_msg; + struct xbee_msg msg; + uint8_t prio; + int8_t ret; + uint8_t irq_flags; + + imu_msg.type = RC_PROTO_IMU_POS; + + IRQ_LOCK(irq_flags); + imu_msg.valid = !!(imuboard_status.flags & I2C_IMUBOARD_STATUS_IMU_OK); + imu_msg.roll = imuboard_status.roll; + imu_msg.pitch = imuboard_status.pitch; + imu_msg.yaw = imuboard_status.yaw; + IRQ_UNLOCK(irq_flags); + + msg.iovlen = 1; + msg.iov[0].buf = &imu_msg; + msg.iov[0].len = sizeof(imu_msg); + + /* set power level */ + if (power != -1) + xbeeapp_send_atcmd("PL", &power, sizeof(power), NULL, NULL); + + /* we also need to lock callout to increment stats */ + prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO); + stats.imu_pos_tx++; + ret = xbeeapp_send_msg(addr, &msg, NULL, NULL); + callout_mgr_restore_prio(&xbeeboard.intr_cm, prio); + + return ret; +} + + + +void rc_proto_set_mode(uint16_t flags) { rc_proto_flags = flags; } -uint8_t rc_proto_get_mode(void) +uint16_t rc_proto_get_mode(void) { return rc_proto_flags; } @@ -691,6 +771,40 @@ int rc_proto_rx(struct xbee_recv_hdr *recvframe, unsigned len) return 0; } + /* received by the radio controller */ + case RC_PROTO_GPS_POS: { + struct rc_proto_gps_pos *rcmsg = + (struct rc_proto_gps_pos *) recvframe->data; + + NOTICE(E_USER_XBEE, "recv gps_pos"); + + if (datalen != sizeof(*rcmsg)) + return -1; + + stats.gps_pos_rx++; + printf_P(PSTR("GPS received %"PRIu32" %" + PRIu32" %"PRIu32"\n"), + rcmsg->latitude, rcmsg->longitude, + rcmsg->altitude); + return 0; + } + + /* received by the radio controller */ + case RC_PROTO_IMU_POS: { + struct rc_proto_imu_pos *rcmsg = + (struct rc_proto_imu_pos *) recvframe->data; + + NOTICE(E_USER_XBEE, "recv imu_pos"); + + if (datalen != sizeof(*rcmsg)) + return -1; + + stats.imu_pos_rx++; + printf_P(PSTR("IMU received %d %d %d\n"), + rcmsg->roll, rcmsg->pitch, rcmsg->yaw); + return 0; + } + default: return -1; } @@ -706,6 +820,8 @@ static void rc_proto_cb(struct callout_mgr *cm, struct callout *tim, void *arg) static uint16_t prev_stats_send; static uint16_t prev_compute_pow; static uint16_t prev_power_probe; + static uint16_t prev_gps_pos; + static uint16_t prev_imu_pos; static uint8_t pow_probe; uint16_t t, diff; @@ -727,6 +843,30 @@ static void rc_proto_cb(struct callout_mgr *cm, struct callout *tim, void *arg) } } + /* send gps periodically */ + if (rc_proto_flags & RC_PROTO_FLAGS_TX_GPS_POS) { + diff = t - prev_gps_pos; + if (diff > rc_proto_timers.send_gps_pos_ms) { + pow_probe++; + if (pow_probe > 4) + pow_probe = 0; + rc_proto_send_gps_pos(rc_proto_dstaddr, pow_probe); + prev_gps_pos = t; + } + } + + /* send imu periodically */ + if (rc_proto_flags & RC_PROTO_FLAGS_TX_IMU_POS) { + diff = t - prev_imu_pos; + if (diff > rc_proto_timers.send_imu_pos_ms) { + pow_probe++; + if (pow_probe > 4) + pow_probe = 0; + rc_proto_send_imu_pos(rc_proto_dstaddr, pow_probe); + prev_imu_pos = 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; @@ -755,47 +895,39 @@ static void rc_proto_cb(struct callout_mgr *cm, struct callout *tim, void *arg) callout_schedule(cm, tim, 0); } -void rc_proto_dump_stats(void) +static void __dump_stats(struct rc_proto_stats_data *s) { - 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(" hello_tx: %"PRIu32"\r\n"), s->hello_tx); + printf_P(PSTR(" hello_rx: %"PRIu32"\r\n"), s->hello_rx); + printf_P(PSTR(" echo_req_rx: %"PRIu32"\r\n"), s->echo_req_rx); + printf_P(PSTR(" echo_req_tx: %"PRIu32"\r\n"), s->echo_req_tx); + printf_P(PSTR(" echo_ans_rx: %"PRIu32"\r\n"), s->echo_ans_rx); + printf_P(PSTR(" echo_ans_tx: %"PRIu32"\r\n"), s->echo_ans_tx); + printf_P(PSTR(" power_probe_rx: %"PRIu32"\r\n"), s->power_probe_rx); + printf_P(PSTR(" power_probe_tx: %"PRIu32"\r\n"), s->power_probe_tx); + printf_P(PSTR(" ack_rx: %"PRIu32"\r\n"), s->ack_rx); + printf_P(PSTR(" ack_tx: %"PRIu32"\r\n"), s->ack_tx); + printf_P(PSTR(" servo_rx: %"PRIu32"\r\n"), s->servo_rx); + printf_P(PSTR(" servo_tx: %"PRIu32"\r\n"), s->servo_tx); + printf_P(PSTR(" stats_rx: %"PRIu32"\r\n"), s->stats_rx); + printf_P(PSTR(" stats_tx: %"PRIu32"\r\n"), s->stats_tx); + printf_P(PSTR(" gps_pos_rx: %"PRIu32"\r\n"), s->gps_pos_rx); + printf_P(PSTR(" gps_pos_tx: %"PRIu32"\r\n"), s->gps_pos_tx); + printf_P(PSTR(" imu_pos_rx: %"PRIu32"\r\n"), s->imu_pos_rx); + printf_P(PSTR(" imu_pos_tx: %"PRIu32"\r\n"), s->imu_pos_tx); + if (s->echo_ans_rx != 0) { printf_P(PSTR(" echo_ans_latency_ms: %"PRIu32"\r\n"), - stats.echo_ans_latency_sum / stats.echo_ans_rx); + s->echo_ans_latency_sum / s->echo_ans_rx); } +} + +void rc_proto_dump_stats(void) +{ + printf_P(PSTR("rc_proto stats LOCAL\r\n")); + __dump_stats(&stats); 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); - } + __dump_stats(&peer_stats); } void rc_proto_reset_stats(void) diff --git a/mainboard/rc_proto.h b/mainboard/rc_proto.h index baf90f4..3ffe156 100644 --- a/mainboard/rc_proto.h +++ b/mainboard/rc_proto.h @@ -86,11 +86,42 @@ struct rc_proto_stats { uint8_t stats[]; /* format is struct rc_proto_stats_data */ } __attribute__((packed)); -/* rc_proto timers configuration */ +/* + * GPS position + */ +#define RC_PROTO_GPS_POS 0x07 +struct rc_proto_gps_pos { + uint8_t type; + + uint8_t valid; /* 1 if position is valid */ + int32_t latitude; /* between -90e7 and 90e7, in 1/1e-7 degrees, + * positive means north hemisphere */ + int32_t longitude; /* between -180e7 and 180e7, in 1/1e-7 degrees, + * positive means east */ + uint32_t altitude; /* altitude from elipsoid, in 1/100 meters */ +}; + +/* + * IMU position + */ +#define RC_PROTO_IMU_POS 0x08 +struct rc_proto_imu_pos { + uint8_t type; + + uint8_t valid; + uint16_t roll; /* XXX unit ? */ + uint16_t pitch; + uint16_t yaw; +}; + + +/* rc_proto timers configuration (default values in rc_proto.c) */ struct rc_proto_timers { uint16_t send_servo_min_ms; uint16_t send_servo_max_ms; uint16_t send_power_probe_ms; + uint16_t send_gps_pos_ms; + uint16_t send_imu_pos_ms; uint16_t autobypass_ms; }; extern struct rc_proto_timers rc_proto_timers; @@ -120,30 +151,36 @@ 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 +#define RC_PROTO_FLAGS_TX_OFF 0x0000 +#define RC_PROTO_FLAGS_TX_BYPASS 0x0001 +#define RC_PROTO_FLAGS_TX_COPY_SPI 0x0002 +#define RC_PROTO_FLAGS_TX_RESERVED 0x0003 +#define RC_PROTO_FLAGS_TX_MASK 0x0003 /* if set, copy received servo values to SPI */ -#define RC_PROTO_FLAGS_RX_COPY_SPI 0x04 +#define RC_PROTO_FLAGS_RX_COPY_SPI 0x0004 /* if set, switch to bypass when no servo is received during some time */ -#define RC_PROTO_FLAGS_RX_AUTOBYPASS 0x08 +#define RC_PROTO_FLAGS_RX_AUTOBYPASS 0x0008 /* if set, send stats periodically to the peer (1 sec) */ -#define RC_PROTO_FLAGS_TX_STATS 0x10 +#define RC_PROTO_FLAGS_TX_STATS 0x0010 /* if set, send power probe periodically to the peer (500 ms) */ -#define RC_PROTO_FLAGS_TX_POW_PROBE 0x20 +#define RC_PROTO_FLAGS_TX_POW_PROBE 0x0020 + +/* if set, send GPS position to the peer (500 ms) */ +#define RC_PROTO_FLAGS_TX_GPS_POS 0x0040 + +/* if set, send IMU position to the peer (100 ms) */ +#define RC_PROTO_FLAGS_TX_IMU_POS 0x0080 /* if set, use received probes to compute best power level */ -#define RC_PROTO_FLAGS_COMPUTE_BEST_POW 0x40 +#define RC_PROTO_FLAGS_COMPUTE_BEST_POW 0x0100 -void rc_proto_set_mode(uint8_t flags); +void rc_proto_set_mode(uint16_t flags); -uint8_t rc_proto_get_mode(void); +uint16_t rc_proto_get_mode(void); /* initialize rc_proto module */ void rc_proto_init(void); -- 2.20.1