aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--makefiles/config_px4fmu-v1_default.mk8
-rw-r--r--src/drivers/mpu6000/mpu6000.cpp17
-rw-r--r--src/drivers/px4io/px4io.cpp9
-rw-r--r--src/lib/conversion/module.mk2
-rw-r--r--src/modules/commander/commander.cpp23
-rw-r--r--src/modules/navigator/navigator_main.cpp5
-rw-r--r--src/modules/systemlib/circuit_breaker.c16
-rw-r--r--src/modules/systemlib/circuit_breaker.h1
-rw-r--r--src/modules/uORB/topics/vehicle_status.h2
9 files changed, 55 insertions, 28 deletions
diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk
index ffb9391d2..9aa8320ee 100644
--- a/makefiles/config_px4fmu-v1_default.mk
+++ b/makefiles/config_px4fmu-v1_default.mk
@@ -24,16 +24,16 @@ 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 += drivers/frsky_telemetry
MODULES += modules/sensors
#
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index 6f5dae7ad..b22bb2e07 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);
@@ -968,11 +970,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);
@@ -1053,14 +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);
- if (arg == 0) {
- // allow disabling of on-chip filter using 0
- // as desired frequency
- _set_dlpf_filter(0);
- }
return OK;
case GYROIOCSSCALE:
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 6f8d6f5f6..fbb5d4f2e 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;
diff --git a/src/lib/conversion/module.mk b/src/lib/conversion/module.mk
index f5f59a2dc..4593c4887 100644
--- a/src/lib/conversion/module.mk
+++ b/src/lib/conversion/module.mk
@@ -36,3 +36,5 @@
#
SRCS = rotation.cpp
+
+MAXOPTIMIZATION = -Os
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index bf15bbeb6..9ebe006f0 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,8 +1425,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 (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 */
if (status.gps_failure) {
status.gps_failure = false;
@@ -1615,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/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index b81155efe..4e2511e7a 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -430,9 +430,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:
@@ -448,7 +445,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) {
diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c
index 2058fa0d5..8ad79f94d 100644
--- a/src/modules/systemlib/circuit_breaker.c
+++ b/src/modules/systemlib/circuit_breaker.c
@@ -120,7 +120,21 @@ PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212);
* @max 284953
* @group Circuit Breaker
*/
-PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 0);
+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);
/**
* Circuit breaker for gps failure detection
diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h
index 6a55e4948..b3431722f 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 <stdbool.h>
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 9dccb2309..505039d90 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;
};
/**
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-28 15:33:52 +0000