From 6016fbe55d3fd402012168b5a704a550bcfc4d28 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 13 Dec 2013 11:30:07 +1100 Subject: Merged PX4IO crc checks and force update --- src/modules/px4iofirmware/protocol.h | 3 +++ 1 file changed, 3 insertions(+) (limited to 'src/modules/px4iofirmware/protocol.h') diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 5e5396782..2856bdea4 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -186,6 +186,9 @@ enum { /* DSM bind states */ /* 8 */ #define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ +#define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */ +#define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */ + /* autopilot control values, -10000..10000 */ #define PX4IO_PAGE_CONTROLS 51 /* 0..CONFIG_CONTROL_COUNT */ -- cgit v1.2.3 From 5b7d1af5d83554d3119e3e0669d6429fc1aef262 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 13 Dec 2013 12:44:11 +1100 Subject: Merged crccheck command --- src/drivers/px4io/px4io.cpp | 64 +++++++++++++++++++++++++++++++++-- src/modules/px4iofirmware/protocol.h | 2 ++ src/modules/px4iofirmware/px4io.c | 20 +++++++++++ src/modules/px4iofirmware/registers.c | 1 + 4 files changed, 85 insertions(+), 2 deletions(-) (limited to 'src/modules/px4iofirmware/protocol.h') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index f5fb618c3..cc3815f3e 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -54,6 +54,7 @@ #include #include #include +#include #include @@ -96,6 +97,7 @@ extern device::Device *PX4IO_serial_interface() weak_function; #define PX4IO_SET_DEBUG _IOC(0xff00, 0) #define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1) #define PX4IO_REBOOT_BOOTLOADER _IOC(0xff00, 2) +#define PX4IO_CHECK_CRC _IOC(0xff00, 3) #define UPDATE_INTERVAL_MIN 2 // 2 ms -> 500 Hz #define ORB_CHECK_INTERVAL 200000 // 200 ms -> 5 Hz @@ -1660,11 +1662,13 @@ void PX4IO::print_status() { /* basic configuration */ - printf("protocol %u hardware %u bootloader %u buffer %uB\n", + printf("protocol %u hardware %u bootloader %u buffer %uB crc 0x%04x%04x\n", io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION), io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION), io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER)); + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC+1)); printf("%u controls %u actuators %u R/C inputs %u analog inputs %u relays\n", io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT), io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT), @@ -2157,6 +2161,19 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) ret = OK; break; + case PX4IO_CHECK_CRC: { + /* check IO firmware CRC against passed value */ + uint32_t io_crc = 0; + ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC, (uint16_t *)&io_crc, 2); + if (ret != OK) + return ret; + if (io_crc != arg) { + debug("crc mismatch 0x%08x 0x%08x", (unsigned)io_crc, arg); + return -EINVAL; + } + break; + } + case PX4IO_INAIR_RESTART_ENABLE: /* set/clear the 'in-air restart' bit */ @@ -2717,6 +2734,49 @@ px4io_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "checkcrc")) { + /* + check IO CRC against CRC of a file + */ + if (argc <= 2) { + printf("usage: px4io checkcrc filename\n"); + exit(1); + } + if (g_dev == nullptr) { + printf("px4io is not started\n"); + exit(1); + } + int fd = open(argv[2], O_RDONLY); + if (fd == -1) { + printf("open of %s failed - %d\n", argv[2], errno); + exit(1); + } + const uint32_t app_size_max = 0xf000; + uint32_t fw_crc = 0; + uint32_t nbytes = 0; + while (true) { + uint8_t buf[16]; + int n = read(fd, buf, sizeof(buf)); + if (n <= 0) break; + fw_crc = crc32part(buf, n, fw_crc); + nbytes += n; + } + close(fd); + while (nbytes < app_size_max) { + uint8_t b = 0xff; + fw_crc = crc32part(&b, 1, fw_crc); + nbytes++; + } + + int ret = g_dev->ioctl(nullptr, PX4IO_CHECK_CRC, fw_crc); + if (ret != OK) { + printf("check CRC failed - %d\n", ret); + exit(1); + } + printf("CRCs match\n"); + exit(0); + } + if (!strcmp(argv[1], "rx_dsm") || !strcmp(argv[1], "rx_dsm_10bit") || !strcmp(argv[1], "rx_dsm_11bit") || diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 2856bdea4..cffabbb45 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -189,6 +189,8 @@ enum { /* DSM bind states */ #define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */ #define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */ +#define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */ + /* autopilot control values, -10000..10000 */ #define PX4IO_PAGE_CONTROLS 51 /* 0..CONFIG_CONTROL_COUNT */ diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index cd9bd197b..745bd5705 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -45,6 +45,7 @@ #include #include #include +#include #include #include @@ -124,6 +125,22 @@ heartbeat_blink(void) LED_BLUE(heartbeat = !heartbeat); } + +static void +calculate_fw_crc(void) +{ +#define APP_SIZE_MAX 0xf000 +#define APP_LOAD_ADDRESS 0x08001000 + // compute CRC of the current firmware + uint32_t sum = 0; + for (unsigned p = 0; p < APP_SIZE_MAX; p += 4) { + uint32_t bytes = *(uint32_t *)(p + APP_LOAD_ADDRESS); + sum = crc32part((uint8_t *)&bytes, sizeof(bytes), sum); + } + r_page_setup[PX4IO_P_SETUP_CRC] = sum & 0xFFFF; + r_page_setup[PX4IO_P_SETUP_CRC+1] = sum >> 16; +} + int user_start(int argc, char *argv[]) { @@ -136,6 +153,9 @@ user_start(int argc, char *argv[]) /* configure the high-resolution time/callout interface */ hrt_init(); + /* calculate our fw CRC so FMU can decide if we need to update */ + calculate_fw_crc(); + /* * Poll at 1ms intervals for received bytes that have not triggered * a DMA event. diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 6785e5366..1a8519aec 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -157,6 +157,7 @@ volatile uint16_t r_page_setup[] = #endif [PX4IO_P_SETUP_SET_DEBUG] = 0, [PX4IO_P_SETUP_REBOOT_BL] = 0, + [PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0, }; #define PX4IO_P_SETUP_FEATURES_VALID (0) -- cgit v1.2.3 From 9883a346a022131b20a292d96a87f20fb7921b1c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 13 Dec 2013 18:01:55 +0100 Subject: First stab at implementing better RSSI based connection status estimation, still needs some work and testing --- src/drivers/drv_rc_input.h | 3 +++ src/modules/px4iofirmware/controls.c | 21 +++++++++++++++++---- src/modules/px4iofirmware/protocol.h | 1 + src/modules/px4iofirmware/px4io.h | 2 +- src/modules/px4iofirmware/registers.c | 2 +- src/modules/px4iofirmware/sbus.c | 17 ++++++++++------- 6 files changed, 33 insertions(+), 13 deletions(-) (limited to 'src/modules/px4iofirmware/protocol.h') diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index 78ffad9d7..86e5a149a 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -89,6 +89,9 @@ struct rc_input_values { /** number of channels actually being seen */ uint32_t channel_count; + /** receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 1000: full reception */ + int32_t rssi; + /** Input source */ enum RC_INPUT_SOURCE input_source; diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 5c621cfb2..194d8aab9 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -94,6 +94,9 @@ controls_tick() { * other. Don't do that. */ + /* receive signal strenght indicator (RSSI). 0 = no connection, 1000: perfect connection */ + uint16_t rssi = 0; + perf_begin(c_gather_dsm); uint16_t temp_count = r_raw_rc_count; bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count); @@ -104,14 +107,15 @@ controls_tick() { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM11; else r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11; + + rssi = 1000; } perf_end(c_gather_dsm); perf_begin(c_gather_sbus); - bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, PX4IO_CONTROL_CHANNELS /* XXX this should be INPUT channels, once untangled */); + bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &rssi, PX4IO_CONTROL_CHANNELS /* XXX this should be INPUT channels, once untangled */); if (sbus_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; - r_raw_rc_count = 8; } perf_end(c_gather_sbus); @@ -122,10 +126,19 @@ controls_tick() { */ perf_begin(c_gather_ppm); bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count); - if (ppm_updated) + if (ppm_updated) { + + /* XXX sample RSSI properly here */ + rssi = 1000; + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; + } perf_end(c_gather_ppm); + /* limit number of channels to allowable data size */ + if (r_raw_rc_count > PX4IO_INPUT_CHANNELS) + r_raw_rc_count = PX4IO_INPUT_CHANNELS; + /* * In some cases we may have received a frame, but input has still * been lost. @@ -221,7 +234,7 @@ controls_tick() { * This might happen if a protocol-based receiver returns an update * that contains no channels that we have mapped. */ - if (assigned_channels == 0) { + if (assigned_channels == 0 || rssi == 0) { rc_input_lost = true; } else { /* set RC OK flag */ diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index cffabbb45..11e380397 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -124,6 +124,7 @@ #define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */ #define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */ #define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */ +#define PX4IO_P_STATUS_NRSSI 9 /* [2] Normalized RSSI value, 0: no reception, 1000: perfect reception */ /* array of post-mix actuator outputs, -10000..10000 */ #define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 4fea0288c..b9c9e0232 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -209,7 +209,7 @@ extern int dsm_init(const char *device); extern bool dsm_input(uint16_t *values, uint16_t *num_values); extern void dsm_bind(uint16_t cmd, int pulses); extern int sbus_init(const char *device); -extern bool sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels); +extern bool sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels); /** global debug level for isr_debug() */ extern volatile uint8_t debug_level; diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 1a8519aec..3f9e111ba 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -114,7 +114,7 @@ uint16_t r_page_servos[PX4IO_SERVO_COUNT]; uint16_t r_page_raw_rc_input[] = { [PX4IO_P_RAW_RC_COUNT] = 0, - [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0 + [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + 24)] = 0 // XXX ensure we have enough space to decode beefy RX, will be replaced by patch soon }; /** diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 68af85dc9..2153fadc8 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -87,7 +87,7 @@ static unsigned partial_frame_count; unsigned sbus_frame_drops; -static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t max_channels); +static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels); int sbus_init(const char *device) @@ -118,7 +118,7 @@ sbus_init(const char *device) } bool -sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels) +sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels) { ssize_t ret; hrt_abstime now; @@ -175,7 +175,7 @@ sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels) * decode it. */ partial_frame_count = 0; - return sbus_decode(now, values, num_values, max_channels); + return sbus_decode(now, values, num_values, rssi, max_channels); } /* @@ -215,7 +215,7 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { }; static bool -sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t max_values) +sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_values) { /* check frame boundary markers to avoid out-of-sync cases */ if ((frame[0] != 0x0f) || (frame[24] != 0x00)) { @@ -266,8 +266,9 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint /* decode and handle failsafe and frame-lost flags */ if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */ - /* emulate throttle failsafe for maximum compatibility and do not destroy any channel data */ - values[2] = 900; + /* report that we failed to read anything valid off the receiver */ + *rssi = 0; + return false; } else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */ /* set a special warning flag or try to calculate some kind of RSSI information - to be implemented @@ -276,8 +277,10 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint * condition as fail-safe greatly reduces the reliability and range of the radio link, * e.g. by prematurely issueing return-to-launch!!! */ - // values[2] = 888; // marker for debug purposes + *rssi = 100; // XXX magic number indicating bad signal, but not a signal loss (yet) } + *rssi = 1000; + return true; } -- cgit v1.2.3