From 3e1eec5906ea8df955bf6d0ac7ec182979f47c90 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 10 Sep 2014 14:42:32 +0900 Subject: [PATCH 1/9] mpu6k: set hardware filter during ACCELIOCLOWPASS also set from GYROIOCLOWPASS Conflicts: mavlink/include/mavlink/v1.0 --- src/drivers/mpu6000/mpu6000.cpp | 15 +++++---------- 1 file changed, 5 insertions(+), 10 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 6f5dae7adb..fb38c7e67b 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -968,11 +968,9 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) return _accel_filter_x.get_cutoff_freq(); case ACCELIOCSLOWPASS: - if (arg == 0) { - // allow disabling of on-chip filter using - // zero as desired filter frequency - _set_dlpf_filter(0); - } + // set hardware filtering + _set_dlpf_filter(arg); + // set software filtering _accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); _accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); _accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); @@ -1056,11 +1054,8 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) _gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); _gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); _gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); - if (arg == 0) { - // allow disabling of on-chip filter using 0 - // as desired frequency - _set_dlpf_filter(0); - } + // set hardware filtering + _set_dlpf_filter(arg); return OK; case GYROIOCSSCALE: From 8ced6bb49bf32128d2673e565cb112ca6d2f21eb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Sep 2014 10:05:39 +0200 Subject: [PATCH 2/9] Set filter frequency for hardware and software in parallel, always do so in the same order --- src/drivers/mpu6000/mpu6000.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index fb38c7e67b..b22bb2e070 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -910,12 +910,14 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) // adjust filters float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); float sample_rate = 1.0e6f/ticks; + _set_dlpf_filter(cutoff_freq_hz); _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq(); + _set_dlpf_filter(cutoff_freq_hz_gyro); _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); @@ -1051,11 +1053,11 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCGLOWPASS: return _gyro_filter_x.get_cutoff_freq(); case GYROIOCSLOWPASS: + // set hardware filtering + _set_dlpf_filter(arg); _gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); _gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); _gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); - // set hardware filtering - _set_dlpf_filter(arg); return OK; case GYROIOCSSCALE: From 70606d400bdceaa86331670938c1e0ae988ed97c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 11:11:30 +0200 Subject: [PATCH 3/9] remove wrong comments --- src/modules/navigator/navigator_main.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index fad46998e1..bf5e36d39e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -432,9 +432,6 @@ Navigator::task_main() _can_loiter_at_sp = false; break; case NAVIGATION_STATE_AUTO_MISSION: - /* Some failsafe modes prohibit the fallback to mission - * usually this is done after some time to make sure - * that the full failsafe operation is performed */ _navigation_mode = &_mission; break; case NAVIGATION_STATE_AUTO_LOITER: @@ -450,7 +447,7 @@ Navigator::task_main() case NAVIGATION_STATE_AUTO_RTL: _navigation_mode = &_rtl; break; - case NAVIGATION_STATE_AUTO_RTGS: //XXX OBC: differentiate between rc loss and dl loss here + case NAVIGATION_STATE_AUTO_RTGS: /* Use complex data link loss mode only when enabled via param * otherwise use rtl */ if (_param_datalinkloss_obc.get() != 0) { From 1072a3380c2d6bdea010bb5091c6d0b23fe6f224 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 11:11:46 +0200 Subject: [PATCH 4/9] enable engine failure circuit breaker --- src/modules/systemlib/circuit_breaker.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index 64420be379..83c8344d54 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -120,7 +120,7 @@ PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212); * @max 284953 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 0); +PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953); bool circuit_breaker_enabled(const char* breaker, int32_t magic) { From d4c0dc2ba0271f4d9c8044fd2a3a178cbb9987e3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 11:20:30 +0200 Subject: [PATCH 5/9] add and activate circuit breaker for gps failure detection --- src/modules/commander/commander.cpp | 5 +++-- src/modules/systemlib/circuit_breaker.c | 14 ++++++++++++++ src/modules/systemlib/circuit_breaker.h | 1 + 3 files changed, 18 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bf15bbeb60..b86f3678b0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1417,8 +1417,9 @@ int commander_thread_main(int argc, char *argv[]) } /* check if GPS fix is ok */ - if (gps_position.fix_type >= 3 && //XXX check eph and epv ? - hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT) { + if (circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY) || + (gps_position.fix_type >= 3 && + hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT)) { /* handle the case where gps was regained */ if (status.gps_failure) { status.gps_failure = false; diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index 83c8344d54..12187d70ea 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -122,6 +122,20 @@ PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212); */ PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953); +/** + * Circuit breaker for gps failure detection + * + * Setting this parameter to 240024 will disable the gps failure detection. + * If the aircraft is in gps failure mode the gps failure flag will be + * set to healthy + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 240024 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024); + bool circuit_breaker_enabled(const char* breaker, int32_t magic) { int32_t val; diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index 6a55e4948d..b3431722fc 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -55,6 +55,7 @@ #define CBRK_AIRSPD_CHK_KEY 162128 #define CBRK_FLIGHTTERM_KEY 121212 #define CBRK_ENGINEFAIL_KEY 284953 +#define CBRK_GPSFAIL_KEY 240024 #include From 8b7c57a0d050d51f45f8c66536e5450e7a494d73 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 13:39:40 +0200 Subject: [PATCH 6/9] px4io driver: update cb only when changed --- src/drivers/px4io/px4io.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 6f8d6f5f6b..fbb5d4f2e6 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -295,6 +295,7 @@ private: float _battery_amp_bias; ///< current sensor bias float _battery_mamphour_total;///< amp hours consumed so far uint64_t _battery_last_timestamp;///< last amp hour calculation timestamp + bool _cb_flighttermination; ///< true if the flight termination circuit breaker is enabled #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 bool _dsm_vcc_ctl; ///< true if relay 1 controls DSM satellite RX power @@ -515,7 +516,8 @@ PX4IO::PX4IO(device::Device *interface) : _battery_amp_per_volt(90.0f / 5.0f), // this matches the 3DR current sensor _battery_amp_bias(0), _battery_mamphour_total(0), - _battery_last_timestamp(0) + _battery_last_timestamp(0), + _cb_flighttermination(true) #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 , _dsm_vcc_ctl(false) #endif @@ -1051,6 +1053,9 @@ PX4IO::task_main() } } + /* Update Circuit breakers */ + _cb_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY); + } } @@ -1170,7 +1175,7 @@ PX4IO::io_set_arming_state() } /* Do not set failsafe if circuit breaker is enabled */ - if (armed.force_failsafe && !circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY)) { + if (armed.force_failsafe && !_cb_flighttermination) { set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; } else { clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; From d11f05b12c0b0c56703ab14303f91efd123b9dc6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 13:40:03 +0200 Subject: [PATCH 7/9] commander: update gps and engine cb only when changed --- src/modules/commander/commander.cpp | 20 ++++++++++++++------ src/modules/uORB/topics/vehicle_status.h | 2 ++ 2 files changed, 16 insertions(+), 6 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b86f3678b0..9ebe006f0d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -775,6 +775,8 @@ int commander_thread_main(int argc, char *argv[]) // CIRCUIT BREAKERS status.circuit_breaker_engaged_power_check = false; status.circuit_breaker_engaged_airspd_check = false; + status.circuit_breaker_engaged_enginefailure_check = false; + status.circuit_breaker_engaged_gpsfailure_check = false; /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); @@ -980,8 +982,8 @@ int commander_thread_main(int argc, char *argv[]) int32_t ef_throttle_thres = 1.0f; int32_t ef_current2throttle_thres = 0.0f; int32_t ef_time_thres = 1000.0f; - uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine - was healty*/ + uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine was healty */ + /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; bool main_state_changed = false; @@ -1028,8 +1030,14 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_system_id, &(status.system_id)); param_get(_param_component_id, &(status.component_id)); - status.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY); - status.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY); + status.circuit_breaker_engaged_power_check = + circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY); + status.circuit_breaker_engaged_airspd_check = + circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY); + status.circuit_breaker_engaged_enginefailure_check = + circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY); + status.circuit_breaker_engaged_gpsfailure_check = + circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY); status_changed = true; @@ -1417,7 +1425,7 @@ int commander_thread_main(int argc, char *argv[]) } /* check if GPS fix is ok */ - if (circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY) || + if (status.circuit_breaker_engaged_gpsfailure_check || (gps_position.fix_type >= 3 && hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT)) { /* handle the case where gps was regained */ @@ -1616,7 +1624,7 @@ int commander_thread_main(int argc, char *argv[]) /* Check engine failure * only for fixed wing for now */ - if (!circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY) && + if (!status.circuit_breaker_engaged_enginefailure_check && status.is_rotary_wing == false && armed.armed && ((actuator_controls.control[3] > ef_throttle_thres && diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 9dccb2309d..505039d90d 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -239,6 +239,8 @@ struct vehicle_status_s { bool circuit_breaker_engaged_power_check; bool circuit_breaker_engaged_airspd_check; + bool circuit_breaker_engaged_enginefailure_check; + bool circuit_breaker_engaged_gpsfailure_check; }; /** From 47dcf88271b4b05f2007383e62a6dd239b7eb859 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Sep 2014 15:18:30 +0200 Subject: [PATCH 8/9] Flash optimization --- makefiles/config_px4fmu-v1_default.mk | 6 +++--- src/lib/conversion/module.mk | 2 ++ 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 6f39b56a13..cf631c2be5 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -24,14 +24,14 @@ MODULES += drivers/l3gd20 MODULES += drivers/mpu6000 MODULES += drivers/hmc5883 MODULES += drivers/ms5611 -MODULES += drivers/mb12xx +#MODULES += drivers/mb12xx MODULES += drivers/gps MODULES += drivers/hil -MODULES += drivers/blinkm +#MODULES += drivers/blinkm MODULES += drivers/rgbled MODULES += drivers/mkblctrl MODULES += drivers/airspeed -MODULES += drivers/ets_airspeed +#MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed MODULES += drivers/frsky_telemetry MODULES += modules/sensors diff --git a/src/lib/conversion/module.mk b/src/lib/conversion/module.mk index f5f59a2dca..4593c48871 100644 --- a/src/lib/conversion/module.mk +++ b/src/lib/conversion/module.mk @@ -36,3 +36,5 @@ # SRCS = rotation.cpp + +MAXOPTIMIZATION = -Os From 47f151d4cefa5d7a9da34d6139aa493259f8b2fd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Sep 2014 15:19:06 +0200 Subject: [PATCH 9/9] Deactivate FrSky telem by default --- makefiles/config_px4fmu-v1_default.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index cf631c2be5..9fe16fbb6c 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -33,7 +33,7 @@ MODULES += drivers/mkblctrl MODULES += drivers/airspeed #MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed -MODULES += drivers/frsky_telemetry +#MODULES += drivers/frsky_telemetry MODULES += modules/sensors #