From 1a13e66aab3cd88b5447448b577fc44165ab01bc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 7 Jan 2014 15:59:18 +0800 Subject: px4iofirmware: make forceupdate more reliable this schedules a reboot rather than rebooting immediately, which means the FMU gets an ACK for its reboot operation, preventing it from timing out waiting for the ACK. That makes the timing of the reboot more consistent, which makes it more reliable for forceupdate --- src/modules/px4iofirmware/px4io.c | 21 +++++++++++++++++++++ src/modules/px4iofirmware/px4io.h | 4 ++++ src/modules/px4iofirmware/registers.c | 15 +++++---------- 3 files changed, 30 insertions(+), 10 deletions(-) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 745bd5705..0b8c4a6a8 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -125,6 +125,25 @@ heartbeat_blink(void) LED_BLUE(heartbeat = !heartbeat); } +static uint64_t reboot_time; + +/** + schedule a reboot in time_delta_usec microseconds + */ +void schedule_reboot(uint32_t time_delta_usec) +{ + reboot_time = hrt_absolute_time() + time_delta_usec; +} + +/** + check for a scheduled reboot + */ +static void check_reboot(void) +{ + if (reboot_time != 0 && hrt_absolute_time() > reboot_time) { + up_systemreset(); + } +} static void calculate_fw_crc(void) @@ -249,6 +268,8 @@ user_start(int argc, char *argv[]) heartbeat_blink(); } + check_reboot(); + #if 0 /* check for debug activity */ show_debug_messages(); diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index dea04a663..a0daa97ea 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -220,3 +220,7 @@ extern volatile uint8_t debug_level; /** send a debug message to the console */ extern void isr_debug(uint8_t level, const char *fmt, ...); + +/** schedule a reboot */ +extern void schedule_reboot(uint32_t time_delta_usec); + diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 0358725db..ad4473073 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -518,16 +518,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) // check the magic value if (value != PX4IO_REBOOT_BL_MAGIC) break; - - // note that we don't set BL_WAIT_MAGIC in - // BKP_DR1 as that is not necessary given the - // timing of the forceupdate command. The - // bootloader on px4io waits for enough time - // anyway, and this method works with older - // bootloader versions (tested with both - // revision 3 and revision 4). - - up_systemreset(); + + // we schedule a reboot rather than rebooting + // immediately to allow the IO board to ACK + // the reboot command + schedule_reboot(100000); break; case PX4IO_P_SETUP_DSM: -- cgit v1.2.3 From c4fc730acad12b74f51d9ba7d3ff267e3e1a1ab3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 7 Jan 2014 11:13:51 +0800 Subject: FMUv2: make all UARTs use 512 byte buffer, 2048 for CDCACM output this is important when using UARTs for things like secondary GPS modules, which may produce large enough transfers that 128 bytes is not enough. The 2048 buffer for CDCACM transmit makes mavlink log and parameter transfer faster --- nuttx-configs/px4fmu-v2/nsh/defconfig | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 110bcb363..e20411bca 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -582,8 +582,8 @@ CONFIG_USART3_OFLOWCONTROL=y # # UART4 Configuration # -CONFIG_UART4_RXBUFSIZE=128 -CONFIG_UART4_TXBUFSIZE=128 +CONFIG_UART4_RXBUFSIZE=512 +CONFIG_UART4_TXBUFSIZE=512 CONFIG_UART4_BAUD=57600 CONFIG_UART4_BITS=8 CONFIG_UART4_PARITY=0 @@ -594,8 +594,8 @@ CONFIG_UART4_2STOP=0 # # USART6 Configuration # -CONFIG_USART6_RXBUFSIZE=256 -CONFIG_USART6_TXBUFSIZE=256 +CONFIG_USART6_RXBUFSIZE=512 +CONFIG_USART6_TXBUFSIZE=512 CONFIG_USART6_BAUD=57600 CONFIG_USART6_BITS=8 CONFIG_USART6_PARITY=0 @@ -662,7 +662,7 @@ CONFIG_CDCACM_NWRREQS=4 CONFIG_CDCACM_NRDREQS=4 CONFIG_CDCACM_BULKIN_REQLEN=96 CONFIG_CDCACM_RXBUFSIZE=512 -CONFIG_CDCACM_TXBUFSIZE=512 +CONFIG_CDCACM_TXBUFSIZE=2048 CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_PRODUCTID=0x0011 CONFIG_CDCACM_VENDORSTR="3D Robotics" -- cgit v1.2.3 From d4d2571161530848f2a4ac153f2529ab50ec4fcc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 7 Jan 2014 11:25:09 +0800 Subject: FMUv2: enable RXDMA on 2nd GPS port and debug console This should reduce the chance of lost data on these ports due to high interrupt latency --- nuttx-configs/px4fmu-v2/nsh/defconfig | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index e20411bca..8a282693f 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -295,7 +295,7 @@ CONFIG_STM32_USART=y # U[S]ART Configuration # # CONFIG_USART1_RS485 is not set -# CONFIG_USART1_RXDMA is not set +CONFIG_USART1_RXDMA=y # CONFIG_USART2_RS485 is not set CONFIG_USART2_RXDMA=y # CONFIG_USART3_RS485 is not set @@ -304,7 +304,7 @@ CONFIG_USART3_RXDMA=y CONFIG_UART4_RXDMA=y # CONFIG_UART5_RXDMA is not set # CONFIG_USART6_RS485 is not set -# CONFIG_USART6_RXDMA is not set +CONFIG_USART6_RXDMA=y # CONFIG_UART7_RS485 is not set # CONFIG_UART7_RXDMA is not set # CONFIG_UART8_RS485 is not set -- cgit v1.2.3 From 1f564a95ee89278b3e4eea6c2d4ded378b71542c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 7 Jan 2014 11:37:40 +0800 Subject: meas_airspeed: avoid trivial dependency on math lib including the math lib adds a huge amount to flash usage --- src/drivers/meas_airspeed/meas_airspeed.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 3cd6d6720..a95c4576b 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -198,7 +198,9 @@ MEASAirspeed::collect() // uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f)); const float P_min = -1.0f; const float P_max = 1.0f; - float diff_press_pa = math::max(0.0f, fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset); + float diff_press_pa = fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset; + if (diff_press_pa < 0.0f) + diff_press_pa = 0.0f; struct differential_pressure_s report; -- cgit v1.2.3 From 3be1a5182db7bd3802b77e7c03fc14f00ca218c3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 31 Dec 2013 22:39:51 +1100 Subject: FMUv1: use larger CDCACM buffer size for faster log transfer on FMUv1 --- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index e43b9c18e..e60120b49 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -566,7 +566,7 @@ CONFIG_CDCACM_NWRREQS=4 CONFIG_CDCACM_NRDREQS=4 CONFIG_CDCACM_BULKIN_REQLEN=96 CONFIG_CDCACM_RXBUFSIZE=512 -CONFIG_CDCACM_TXBUFSIZE=512 +CONFIG_CDCACM_TXBUFSIZE=2048 CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_PRODUCTID=0x0010 CONFIG_CDCACM_VENDORSTR="3D Robotics" -- cgit v1.2.3 From d6088efd34e5482d302e3a253fdd3a170d88490a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 31 Dec 2013 10:32:15 +1100 Subject: ms5611: report P and T in ms5611 info --- src/drivers/ms5611/ms5611.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 87788824a..6326cf7fc 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -124,6 +124,8 @@ protected: int32_t _TEMP; int64_t _OFF; int64_t _SENS; + float _P; + float _T; /* altitude conversion calibration */ unsigned _msl_pressure; /* in kPa */ @@ -623,6 +625,8 @@ MS5611::collect() /* pressure calculation, result in Pa */ int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15; + _P = P * 0.01f; + _T = _TEMP * 0.01f; /* generate a new report */ report.temperature = _TEMP / 100.0f; @@ -695,6 +699,8 @@ MS5611::print_info() printf("TEMP: %d\n", _TEMP); printf("SENS: %lld\n", _SENS); printf("OFF: %lld\n", _OFF); + printf("P: %.3f\n", _P); + printf("T: %.3f\n", _T); printf("MSL pressure: %10.4f\n", (double)(_msl_pressure / 100.f)); printf("factory_setup %u\n", _prom.factory_setup); -- cgit v1.2.3 From 94b539dfddc5a2e293f51058ee5bf0d6ffc78406 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 18 Sep 2013 10:30:09 +1000 Subject: px4io: enable power on Spektrum connector on init --- src/modules/px4iofirmware/dsm.c | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index 4d306d6d0..60eda2319 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -203,6 +203,12 @@ dsm_guess_format(bool reset) int dsm_init(const char *device) { + +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 + // enable power on DSM connector + POWER_SPEKTRUM(true); +#endif + if (dsm_fd < 0) dsm_fd = open(device, O_RDONLY | O_NONBLOCK); -- cgit v1.2.3 From 184f4a29eb010413a27cabedbcbc348ef1af7100 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 8 Jan 2014 18:06:30 +0100 Subject: Improved file test, allowed abortion --- src/systemcmds/tests/test_file.c | 50 +++++++++++++++++++++++++++++++++++++++- 1 file changed, 49 insertions(+), 1 deletion(-) diff --git a/src/systemcmds/tests/test_file.c b/src/systemcmds/tests/test_file.c index 798724cf1..cdb0b0337 100644 --- a/src/systemcmds/tests/test_file.c +++ b/src/systemcmds/tests/test_file.c @@ -38,6 +38,7 @@ */ #include +#include #include #include #include @@ -51,6 +52,38 @@ #include "tests.h" +int check_user_abort(); + +int check_user_abort() { + /* check if user wants to abort */ + char c; + + struct pollfd fds; + int ret; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; + ret = poll(&fds, 1, 0); + + if (ret > 0) { + + read(0, &c, 1); + + switch (c) { + case 0x03: // ctrl-c + case 0x1b: // esc + case 'c': + case 'q': + { + warnx("Test aborted."); + return OK; + /* not reached */ + } + } + } + + return 1; +} + int test_file(int argc, char *argv[]) { @@ -108,6 +141,9 @@ test_file(int argc, char *argv[]) fsync(fd); //perf_end(wperf); + if (!check_user_abort()) + return OK; + } end = hrt_absolute_time(); @@ -142,6 +178,9 @@ test_file(int argc, char *argv[]) errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR"); } + if (!check_user_abort()) + return OK; + } /* @@ -152,7 +191,7 @@ test_file(int argc, char *argv[]) int ret = unlink("/fs/microsd/testfile"); fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); - warnx("testing aligned writes - please wait.."); + warnx("testing aligned writes - please wait.. (CTRL^C to abort)"); start = hrt_absolute_time(); for (unsigned i = 0; i < iterations; i++) { @@ -162,6 +201,9 @@ test_file(int argc, char *argv[]) err(1, "WRITE ERROR!"); } + if (!check_user_abort()) + return OK; + } fsync(fd); @@ -190,6 +232,9 @@ test_file(int argc, char *argv[]) align_read_ok = false; break; } + + if (!check_user_abort()) + return OK; } if (!align_read_ok) { @@ -228,6 +273,9 @@ test_file(int argc, char *argv[]) if (unalign_read_err_count > 10) break; } + + if (!check_user_abort()) + return OK; } if (!unalign_read_ok) { -- cgit v1.2.3