FailureDetector (safe version)

ROMFS/px4fmu_common/init.d/rc.interface

@@ -266,6 +266,18 @@ then
 		then
 			pwm failsafe -c ${PWM_AUX_OUT} -p ${FAILSAFE} -d ${OUTPUT_AUX_DEV}
 		fi
+
+		#
+		# Per channel failsafe settings.
+		#
+		pwm failsafe -c 1 -p p:PWM_AUX_FAIL1 -d ${OUTPUT_AUX_DEV}
+		pwm failsafe -c 2 -p p:PWM_AUX_FAIL2 -d ${OUTPUT_AUX_DEV}
+		pwm failsafe -c 3 -p p:PWM_AUX_FAIL3 -d ${OUTPUT_AUX_DEV}
+		pwm failsafe -c 4 -p p:PWM_AUX_FAIL4 -d ${OUTPUT_AUX_DEV}
+		pwm failsafe -c 5 -p p:PWM_AUX_FAIL5 -d ${OUTPUT_AUX_DEV}
+		pwm failsafe -c 6 -p p:PWM_AUX_FAIL6 -d ${OUTPUT_AUX_DEV}
+		pwm failsafe -c 7 -p p:PWM_AUX_FAIL7 -d ${OUTPUT_AUX_DEV}
+		pwm failsafe -c 8 -p p:PWM_AUX_FAIL8 -d ${OUTPUT_AUX_DEV}
 	fi
 fi
 
@@ -310,6 +322,18 @@ then
 	then
 		pwm failsafe -c ${PWM_OUT} -p ${FAILSAFE} -d ${OUTPUT_DEV}
 	fi
+
+	#
+	# Per channel failsafe settings.
+	#
+	pwm failsafe -c 1 -p p:PWM_MAIN_FAIL1
+	pwm failsafe -c 2 -p p:PWM_MAIN_FAIL2
+	pwm failsafe -c 3 -p p:PWM_MAIN_FAIL3
+	pwm failsafe -c 4 -p p:PWM_MAIN_FAIL4
+	pwm failsafe -c 5 -p p:PWM_MAIN_FAIL5
+	pwm failsafe -c 6 -p p:PWM_MAIN_FAIL6
+	pwm failsafe -c 7 -p p:PWM_MAIN_FAIL7
+	pwm failsafe -c 8 -p p:PWM_MAIN_FAIL8
 fi
 
 unset MIXER_AUX_FILE

msg/vehicle_status.msg

@@ -10,6 +10,13 @@ uint8 ARMING_STATE_REBOOT = 4
 uint8 ARMING_STATE_IN_AIR_RESTORE = 5
 uint8 ARMING_STATE_MAX = 6
 
+# FailureDetector status
+uint8 FAILURE_NONE = 0
+uint8 FAILURE_ROLL = 1 	# (1 << 0)
+uint8 FAILURE_PITCH = 2	# (1 << 1)
+uint8 FAILURE_ALT = 4 	# (1 << 2)
+
+# HIL
 uint8 HIL_STATE_OFF = 0
 uint8 HIL_STATE_ON = 1
 
@@ -47,7 +54,7 @@ uint8 RC_IN_MODE_GENERATED = 2
 uint8 nav_state				# set navigation state machine to specified value
 uint8 arming_state			# current arming state
 uint8 hil_state				# current hil state
-bool failsafe				# true if system is in failsafe state
+bool failsafe				# true if system is in failsafe state (e.g.:RTL, Hover, Terminate, ...)
 
 uint8 system_type			# system type, contains mavlink MAV_TYPE
 uint8 system_id			# system id, contains MAVLink's system ID field
@@ -67,6 +74,7 @@ bool high_latency_data_link_active 		# all low latency datalinks to GCS lost
 uint8 data_link_lost_counter			# counts unique data link lost events
 bool engine_failure				# Set to true if an engine failure is detected
 bool mission_failure				# Set to true if mission could not continue/finish
+uint8 failure_detector_status			# Bitmask containing FailureDetector status [0, 0, 0, 0, 0, FAILURE_ALT, FAILURE_PITCH, FAILURE_ROLL]
 
 # see SYS_STATUS mavlink message for the following
 uint32 onboard_control_sensors_present

src/lib/mixer/mixer.cpp

@@ -189,7 +189,7 @@ unsigned
 NullMixer::mix(float *outputs, unsigned space)
 {
 	if (space > 0) {
-		*outputs = 0.0f;
+		*outputs = NAN;
 		return 1;
 	}
 

src/modules/commander/CMakeLists.txt

@@ -52,6 +52,7 @@ px4_add_module(
 		rc_calibration.cpp
 		rc_check.cpp
 		state_machine_helper.cpp
+		failure_detector/FailureDetector.cpp
 	DEPENDS
 		circuit_breaker
 		df_driver_framework

src/modules/commander/Commander.cpp

@@ -540,7 +540,8 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, co
 }
 
 Commander::Commander() :
-	ModuleParams(nullptr)
+	ModuleParams(nullptr),
+	_failure_detector(this)
 {
 	_battery_sub = orb_subscribe(ORB_ID(battery_status));
 }
@@ -2282,6 +2283,32 @@ Commander::run()
 			}
 		}
 
+
+		/* Check for failure detector status */
+		if (armed.armed) {
+
+			if (_failure_detector.update()) {
+
+				const uint8_t failure_status = _failure_detector.get_status();
+
+				if (failure_status != status.failure_detector_status) {
+					status.failure_detector_status = failure_status;
+					status_changed = true;
+				}
+
+				if (failure_status != 0 && !status_flags.circuit_breaker_flight_termination_disabled) {
+
+					// TODO: set force_failsafe flag
+
+					if (!_failure_detector_termination_printed) {
+						mavlink_log_critical(&mavlink_log_pub, "Attitude failure detected: force failsafe");
+						_failure_detector_termination_printed = true;
+					}
+
+				}
+			}
+		}
+
 		/* Get current timestamp */
 		const hrt_abstime now = hrt_absolute_time();
 

src/modules/commander/Commander.hpp

@@ -35,6 +35,7 @@
 #define COMMANDER_HPP_
 
 #include "state_machine_helper.h"
+#include "failure_detector/FailureDetector.hpp"
 
 #include <controllib/blocks.hpp>
 #include <px4_module.h>
@@ -107,7 +108,6 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
 		(ParamInt<px4::params::COM_POS_FS_GAIN>) _failsafe_pos_gain,
 
 		(ParamInt<px4::params::COM_LOW_BAT_ACT>) _low_bat_action
-
 	)
 
 	const int64_t POSVEL_PROBATION_MIN = 1_s;	/**< minimum probation duration (usec) */
@@ -128,6 +128,9 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
 	bool		_nav_test_passed{false};	/**< true if the post takeoff navigation test has passed */
 	bool		_nav_test_failed{false};	/**< true if the post takeoff navigation test has failed */
 
+	FailureDetector _failure_detector;
+	bool _failure_detector_termination_printed{false};
+
 	bool handle_command(vehicle_status_s *status, const vehicle_command_s &cmd,
 			    actuator_armed_s *armed, home_position_s *home, orb_advert_t *home_pub, orb_advert_t *command_ack_pub, bool *changed);
 

src/modules/commander/failure_detector/FailureDetector.cpp

@@ -0,0 +1,97 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+* @file FailureDetector.cpp
+*
+* @author Mathieu Bresciani	<[email protected]>
+*
+*/
+
+#include "FailureDetector.hpp"
+
+FailureDetector::FailureDetector(ModuleParams *parent) :
+	ModuleParams(parent),
+	_sub_vehicle_attitude_setpoint(ORB_ID(vehicle_attitude_setpoint)),
+	_sub_vehicule_attitude(ORB_ID(vehicle_attitude))
+{
+}
+
+bool
+FailureDetector::update()
+{
+	bool updated(false);
+
+	updated = update_attitude_status();
+
+	return updated;
+}
+
+bool
+FailureDetector::update_attitude_status()
+{
+	bool updated(false);
+
+	if (_sub_vehicule_attitude.update()) {
+		const vehicle_attitude_s &attitude = _sub_vehicule_attitude.get();
+
+		const matrix::Eulerf euler(matrix::Quatf(attitude.q));
+		const float roll(euler.phi());
+		const float pitch(euler.theta());
+
+		const float max_roll_deg = _fail_trig_roll.get();
+		const float max_pitch_deg = _fail_trig_pitch.get();
+		const float max_roll(fabsf(math::radians(max_roll_deg)));
+		const float max_pitch(fabsf(math::radians(max_pitch_deg)));
+
+		const bool roll_status = (max_roll > 0.0f) && (fabsf(roll) > max_roll);
+		const bool pitch_status = (max_pitch > 0.0f) && (fabsf(pitch) > max_pitch);
+
+		// Update bitmask
+		_status &= ~(FAILURE_ROLL | FAILURE_PITCH);
+
+		if (roll_status) {
+			_status |= FAILURE_ROLL;
+		}
+		if (pitch_status) {
+			_status |= FAILURE_PITCH;
+		}
+
+		updated = true;
+
+	} else {
+		updated = false;
+	}
+
+	return updated;
+}

src/modules/commander/failure_detector/FailureDetector.hpp

@@ -0,0 +1,88 @@
+
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+* @file FailureDetector.hpp
+* Base class for failure detection logic based on vehicle states
+* for failsafe triggering.
+*
+* @author Mathieu Bresciani 	<[email protected]>
+*
+*/
+
+#pragma once
+
+#include <matrix/matrix/math.hpp>
+#include <mathlib/mathlib.h>
+#include <px4_module_params.h>
+
+// subscriptions
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_status.h>
+
+typedef enum {
+	FAILURE_NONE = vehicle_status_s::FAILURE_NONE,
+	FAILURE_ROLL = vehicle_status_s::FAILURE_ROLL,
+	FAILURE_PITCH = vehicle_status_s::FAILURE_PITCH,
+	FAILURE_ALT = vehicle_status_s::FAILURE_ALT,
+} failure_detector_bitmak;
+
+using uORB::Subscription;
+
+class FailureDetector : public ModuleParams
+{
+public:
+	FailureDetector(ModuleParams *parent);
+
+	bool update();
+
+	uint8_t get_status() const {return _status;}
+
+private:
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::FD_FAIL_P>) _fail_trig_pitch,
+		(ParamInt<px4::params::FD_FAIL_R>) _fail_trig_roll
+	)
+
+	// Subscriptions
+	Subscription<vehicle_attitude_s> _sub_vehicle_attitude_setpoint;
+	Subscription<vehicle_attitude_s> _sub_vehicule_attitude;
+
+	uint8_t _status{FAILURE_NONE};
+
+	bool update_attitude_status();
+};