Merge remote-tracking branch 'upstream/master' into swissfang

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
 
 #

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) {
+		// set hardware filtering
-			// allow disabling of on-chip filter using
+		_set_dlpf_filter(arg);
-			// zero as desired filter frequency
+		// set software filtering
-			_set_dlpf_filter(0);
-		}
 		_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:

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;

src/lib/conversion/module.mk

@@ -36,3 +36,5 @@
 #
 
 SRCS		 = rotation.cpp
+
+MAXOPTIMIZATION	 = -Os

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
+	uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine was healty */
-											 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_power_check =
-				status.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY);
+					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 ?
+		if (status.circuit_breaker_engaged_gpsfailure_check ||
-			hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT) {
+				(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 &&

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) {

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

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>
 

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;
 };
 
 /**