added simple flight termination state machine which enbales parachute on request

6 files changed, 117 insertions, 10 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index dfd4d2f73..40562a4e1 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -509,6 +509,21 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		}
 		break;
 
+	/* Flight termination */
+	case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
+
+			if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
+				transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON, control_mode);
+				result = VEHICLE_CMD_RESULT_ACCEPTED;
+
+			} else {
+				/* reject parachute depoyment not armed */
+				result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+			}
+
+		}
+		break;
+
 	default:
 		break;
 	}
@@ -1199,6 +1214,7 @@ int commander_thread_main(int argc, char *argv[])
 		bool arming_state_changed = check_arming_state_changed();
 		bool main_state_changed = check_main_state_changed();
 		bool navigation_state_changed = check_navigation_state_changed();
+		bool flighttermination_state_changed = check_flighttermination_state_changed();
 
 		hrt_abstime t1 = hrt_absolute_time();
 
@@ -1725,7 +1741,8 @@ void *commander_low_prio_loop(void *arg)
 		/* ignore commands the high-prio loop handles */
 		if (cmd.command == VEHICLE_CMD_DO_SET_MODE ||
 		    cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM ||
-		    cmd.command == VEHICLE_CMD_NAV_TAKEOFF)
+		    cmd.command == VEHICLE_CMD_NAV_TAKEOFF ||
+		    cmd.command == VEHICLE_CMD_DO_SET_SERVO)
 			continue;
 
 		/* only handle low-priority commands here */
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index a50af7daf..6c21dfab0 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -65,6 +65,7 @@ static const int ERROR = -1;
 static bool arming_state_changed = true;
 static bool main_state_changed = true;
 static bool navigation_state_changed = true;
+static bool flighttermination_state_changed = true;
 
 transition_result_t
 arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety,
@@ -451,6 +452,18 @@ check_navigation_state_changed()
 	}
 }
 
+bool
+check_flighttermination_state_changed()
+{
+	if (flighttermination_state_changed) {
+		flighttermination_state_changed = false;
+		return true;
+
+	} else {
+		return false;
+	}
+}
+
 void
 set_navigation_state_changed()
 {
@@ -523,6 +536,44 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 }
 
 
+/**
+* Transition from one flightermination state to another
+*/
+transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state, struct vehicle_control_mode_s *control_mode)
+{
+	transition_result_t ret = TRANSITION_DENIED;
+
+		/* only check transition if the new state is actually different from the current one */
+		if (new_flighttermination_state == status->navigation_state) {
+			ret = TRANSITION_NOT_CHANGED;
+
+		} else {
+
+			switch (new_flighttermination_state) {
+			case FLIGHTTERMINATION_STATE_ON:
+				ret = TRANSITION_CHANGED;
+				status->flighttermination_state = FLIGHTTERMINATION_STATE_ON;
+				warnx("state machine helper: change to FLIGHTTERMINATION_STATE_ON");
+				break;
+			case FLIGHTTERMINATION_STATE_OFF:
+				ret = TRANSITION_CHANGED;
+				status->flighttermination_state = FLIGHTTERMINATION_STATE_OFF;
+				break;
+
+			default:
+				break;
+			}
+
+			if (ret == TRANSITION_CHANGED) {
+				flighttermination_state_changed = true;
+				control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON;
+			}
+		}
+
+		return ret;
+}
+
+
 
 // /*
 //  * Wrapper functions (to be used in the commander), all functions assume lock on current_status
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index 0bfdf36a8..e1ec9d4ad 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -70,8 +70,12 @@ bool check_main_state_changed();
 
 transition_result_t navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode);
 
+transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state, struct vehicle_control_mode_s *control_mode);
+
 bool check_navigation_state_changed();
 
+bool check_flighttermination_state_changed();
+
 void set_navigation_state_changed();
 
 int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd);
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index ff3f13306..bb74534f0 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -121,6 +121,7 @@ private:
 	orb_advert_t	_rate_sp_pub;			/**< rate setpoint publication */
 	orb_advert_t	_attitude_sp_pub;		/**< attitude setpoint point */
 	orb_advert_t	_actuators_0_pub;		/**< actuator control group 0 setpoint */
+	orb_advert_t	_actuators_1_pub;		/**< actuator control group 1 setpoint (Airframe) */
 
 	struct vehicle_attitude_s				_att;			/**< vehicle attitude */
 	struct accel_report						_accel;			/**< body frame accelerations */
@@ -128,7 +129,8 @@ private:
 	struct manual_control_setpoint_s		_manual;		/**< r/c channel data */
 	struct airspeed_s						_airspeed;		/**< airspeed */
 	struct vehicle_control_mode_s			_vcontrol_mode;	/**< vehicle control mode */
-	struct actuator_controls_s				_actuators;		/**< actuator control inputs */
+	struct actuator_controls_s			_actuators;		/**< actuator control inputs */
+	struct actuator_controls_s			_actuators_airframe;		/**< actuator control inputs */
 	struct vehicle_global_position_s		_global_pos;	/**< global position */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
@@ -294,14 +296,15 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_airspeed_valid(false)
 {
 	/* safely initialize structs */
-	vehicle_attitude_s			_att = {0};
-	accel_report				_accel = {0};
-	vehicle_attitude_setpoint_s		_att_sp = {0};
-	manual_control_setpoint_s		_manual = {0};
-	airspeed_s				_airspeed = {0};
-	vehicle_control_mode_s			_vcontrol_mode = {0};
-	actuator_controls_s			_actuators = {0};
-	vehicle_global_position_s		_global_pos = {0};
+	_att = {0};
+	_accel = {0};
+	_att_sp = {0};
+	_manual = {0};
+	_airspeed = {0};
+	_vcontrol_mode = {0};
+	_actuators = {0};
+	_actuators_airframe = {0};
+	_global_pos = {0};
 
 
 	_parameter_handles.tconst = param_find("FW_ATT_TC");
@@ -625,6 +628,15 @@ FixedwingAttitudeControl::task_main()
 				lock_integrator = true;
 			}
 
+			/* Simple handling of failsafe: deploy parachute if failsafe is on */
+			if (_vcontrol_mode.flag_control_flighttermination_enabled) {
+				_actuators_airframe.control[1] = 1.0f;
+//				warnx("_actuators_airframe.control[1] = 1.0f;");
+			} else {
+				_actuators_airframe.control[1] = -1.0f;
+//				warnx("_actuators_airframe.control[1] = -1.0f;");
+			}
+
 			/* decide if in stabilized or full manual control */
 
 			if (_vcontrol_mode.flag_control_attitude_enabled) {
@@ -794,6 +806,7 @@ FixedwingAttitudeControl::task_main()
 
 			/* lazily publish the setpoint only once available */
 			_actuators.timestamp = hrt_absolute_time();
+			_actuators_airframe.timestamp = hrt_absolute_time();
 
 			if (_actuators_0_pub > 0) {
 				/* publish the attitude setpoint */
@@ -804,6 +817,19 @@ FixedwingAttitudeControl::task_main()
 				_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
 			}
 
+			if (_actuators_1_pub > 0) {
+				/* publish the attitude setpoint */
+				orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_airframe);
+				warnx("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f",
+						(double)_actuators_airframe.control[0], (double)_actuators_airframe.control[1], (double)_actuators_airframe.control[2],
+						(double)_actuators_airframe.control[3], (double)_actuators_airframe.control[4], (double)_actuators_airframe.control[5],
+						(double)_actuators_airframe.control[6], (double)_actuators_airframe.control[7]);
+
+			} else {
+				/* advertise and publish */
+				_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_airframe);
+			}
+
 		}
 
 		perf_end(_loop_perf);
diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h
index 38fb74c9b..e26fb97c8 100644
--- a/src/modules/uORB/topics/vehicle_control_mode.h
+++ b/src/modules/uORB/topics/vehicle_control_mode.h
@@ -78,6 +78,7 @@ struct vehicle_control_mode_s
 	bool flag_control_position_enabled;		/**< true if position is controlled */
 	bool flag_control_altitude_enabled;		/**< true if altitude is controlled */
 	bool flag_control_climb_rate_enabled;		/**< true if climb rate is controlled */
+	bool flag_control_flighttermination_enabled;   /**< true if flighttermination is enabled */
 
 	bool flag_control_auto_enabled;		// TEMP
 	uint8_t auto_state;	// TEMP navigation state for AUTO modes
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 1c184d3a7..1c3763924 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -95,6 +95,12 @@ typedef enum {
 	HIL_STATE_ON
 } hil_state_t;
 
+
+typedef enum {
+	FLIGHTTERMINATION_STATE_OFF = 0,
+	FLIGHTTERMINATION_STATE_ON
+} flighttermination_state_t;
+
 typedef enum {
 	MODE_SWITCH_MANUAL = 0,
 	MODE_SWITCH_ASSISTED,
@@ -229,6 +235,8 @@ struct vehicle_status_s
 	uint16_t errors_count2;
 	uint16_t errors_count3;
 	uint16_t errors_count4;
+
+	flighttermination_state_t flighttermination_state;
 };
 
 /**