From 9eea4f79d9588153b751b7ba6053840c94b89194 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 1 Nov 2013 13:58:03 +0400 Subject: [PATCH 1/7] sensors: support for Futaba RC failsafe --- src/modules/sensors/sensor_params.c | 4 ++++ src/modules/sensors/sensors.cpp | 27 +++++++++++++++++++++++++++ 2 files changed, 31 insertions(+) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 587b815886..2aa15420af 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -226,3 +226,7 @@ PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /**< default function: camera azimuth / yaw PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f); PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f); PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f); + +PARAM_DEFINE_INT32(RC_FS_CH, 0); /**< RC failsafe channel, 0 = disable */ +PARAM_DEFINE_INT32(RC_FS_MODE, 0); /**< RC failsafe mode: 0 = too low means signal loss, 1 = too high means signal loss */ +PARAM_DEFINE_FLOAT(RC_FS_THR, 800); /**< RC failsafe PWM threshold */ diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index da0c11372c..21da44d101 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -260,6 +260,10 @@ private: float rc_scale_yaw; float rc_scale_flaps; + int rc_fs_ch; + int rc_fs_mode; + float rc_fs_thr; + float battery_voltage_scaling; } _parameters; /**< local copies of interesting parameters */ @@ -305,6 +309,10 @@ private: param_t rc_scale_yaw; param_t rc_scale_flaps; + param_t rc_fs_ch; + param_t rc_fs_mode; + param_t rc_fs_thr; + param_t battery_voltage_scaling; param_t board_rotation; @@ -517,6 +525,11 @@ Sensors::Sensors() : _parameter_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); _parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS"); + /* RC failsafe */ + _parameter_handles.rc_fs_ch = param_find("RC_FS_CH"); + _parameter_handles.rc_fs_mode = param_find("RC_FS_MODE"); + _parameter_handles.rc_fs_thr = param_find("RC_FS_THR"); + /* gyro offsets */ _parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF"); _parameter_handles.gyro_offset[1] = param_find("SENS_GYRO_YOFF"); @@ -668,6 +681,9 @@ Sensors::parameters_update() param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch)); param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)); param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps)); + param_get(_parameter_handles.rc_fs_ch, &(_parameters.rc_fs_ch)); + param_get(_parameter_handles.rc_fs_mode, &(_parameters.rc_fs_mode)); + param_get(_parameter_handles.rc_fs_thr, &(_parameters.rc_fs_thr)); /* update RC function mappings */ _rc.function[THROTTLE] = _parameters.rc_map_throttle - 1; @@ -1255,6 +1271,17 @@ Sensors::rc_poll() if (rc_input.channel_count < 4) return; + /* failsafe check */ + if (_parameters.rc_fs_ch != 0) { + if (_parameters.rc_fs_mode == 0) { + if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr) + return; + } else if (_parameters.rc_fs_mode == 1) { + if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr) + return; + } + } + unsigned channel_limit = rc_input.channel_count; if (channel_limit > _rc_max_chan_count) From 97acb49028da526ad9081e9eecea97ec0a822858 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 1 Nov 2013 13:58:23 +0400 Subject: [PATCH 2/7] commander: bug fixed in failsafe --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ed090271ca..cbf900fce7 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1600,8 +1600,8 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c /* switch to failsafe mode */ bool manual_control_old = control_mode->flag_control_manual_enabled; - if (!status->condition_landed) { - /* in air: try to hold position */ + if (!status->condition_landed && status->condition_local_position_valid) { + /* in air: try to hold position if possible */ res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode); } else { From af12065826060e3ac071869bf39a82695e1d88e8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 1 Nov 2013 13:59:24 +0400 Subject: [PATCH 3/7] sensors: code style fixed --- src/modules/sensors/sensors.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 21da44d101..3b1460b73b 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1276,6 +1276,7 @@ Sensors::rc_poll() if (_parameters.rc_fs_mode == 0) { if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr) return; + } else if (_parameters.rc_fs_mode == 1) { if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr) return; From d53b00283e4b57611cc7e8c85a4dd1f18629aab0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 16 Dec 2013 12:02:01 +0100 Subject: [PATCH 4/7] PX4IO upgrade improvement --- src/drivers/px4io/px4io.cpp | 66 ++++++++++++++++++------------------- 1 file changed, 33 insertions(+), 33 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 569f1e4137..db882e6232 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2562,6 +2562,39 @@ px4io_main(int argc, char *argv[]) if_test((argc > 2) ? strtol(argv[2], NULL, 0) : 0); } + if (!strcmp(argv[1], "forceupdate")) { + /* + force update of the IO firmware without requiring + the user to hold the safety switch down + */ + if (argc <= 3) { + warnx("usage: px4io forceupdate MAGIC filename"); + exit(1); + } + if (g_dev == nullptr) { + warnx("px4io is not started, still attempting upgrade"); + } else { + uint16_t arg = atol(argv[2]); + int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg); + if (ret != OK) { + printf("reboot failed - %d\n", ret); + exit(1); + } + + // tear down the px4io instance + delete g_dev; + } + + // upload the specified firmware + const char *fn[2]; + fn[0] = argv[3]; + fn[1] = nullptr; + PX4IO_Uploader *up = new PX4IO_Uploader; + up->upload(&fn[0]); + delete up; + exit(0); + } + /* commands below here require a started driver */ if (g_dev == nullptr) @@ -2647,39 +2680,6 @@ px4io_main(int argc, char *argv[]) exit(0); } - if (!strcmp(argv[1], "forceupdate")) { - /* - force update of the IO firmware without requiring - the user to hold the safety switch down - */ - if (argc <= 3) { - warnx("usage: px4io forceupdate MAGIC filename"); - exit(1); - } - if (g_dev == nullptr) { - warnx("px4io is not started, still attempting upgrade"); - } else { - uint16_t arg = atol(argv[2]); - int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg); - if (ret != OK) { - printf("reboot failed - %d\n", ret); - exit(1); - } - - // tear down the px4io instance - delete g_dev; - } - - // upload the specified firmware - const char *fn[2]; - fn[0] = argv[3]; - fn[1] = nullptr; - PX4IO_Uploader *up = new PX4IO_Uploader; - up->upload(&fn[0]); - delete up; - exit(0); - } - if (!strcmp(argv[1], "checkcrc")) { /* check IO CRC against CRC of a file From bccf65cc28edb8e68b38765c895e9f74604df92e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 16 Dec 2013 22:16:51 +1100 Subject: [PATCH 5/7] mpu6000: disable interrupts during initial reset this seems to avoid a problem where the mpu6000 doesn't startup correctly if other devices are transferring at the same time. --- src/drivers/mpu6000/mpu6000.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index be4e422b04..bbc595af46 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -505,17 +505,26 @@ out: void MPU6000::reset() { + // if the mpu6000 is initialised after the l3gd20 and lsm303d + // then if we don't do an irqsave/irqrestore here the mpu6000 + // frequenctly comes up in a bad state where all transfers + // come as zero + irqstate_t state; + state = irqsave(); - // Chip reset write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); up_udelay(10000); - // Wake up device and select GyroZ clock (better performance) + // Wake up device and select GyroZ clock. Note that the + // MPU6000 starts up in sleep mode, and it can take some time + // for it to come out of sleep write_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); up_udelay(1000); // Disable I2C bus (recommended on datasheet) write_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); + irqrestore(state); + up_udelay(1000); // SAMPLE RATE From ea107f41514ac8696ea8b94e93b9231a4802a15b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 16 Dec 2013 16:47:28 +0100 Subject: [PATCH 6/7] Enable DMA on UART8 --- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index db7a9c5c4e..110bcb3637 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -308,7 +308,7 @@ CONFIG_UART4_RXDMA=y # CONFIG_UART7_RS485 is not set # CONFIG_UART7_RXDMA is not set # CONFIG_UART8_RS485 is not set -# CONFIG_UART8_RXDMA is not set +CONFIG_UART8_RXDMA=y CONFIG_SERIAL_DISABLE_REORDERING=y CONFIG_STM32_USART_SINGLEWIRE=y From f042ea162310783a5d58d32478371f2c32a0647e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 17 Dec 2013 10:20:22 +0100 Subject: [PATCH 7/7] Removed whitespace --- ROMFS/px4fmu_common/init.d/rc.sensors | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 7f73acf859..070a4e7e35 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -23,7 +23,7 @@ fi if l3gd20 start then - echo "using L3GD20(H)" + echo "using L3GD20(H)" fi if lsm303d start