Adaptive quadchute

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -132,6 +132,7 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	_params_handles.vtol_type = param_find("VT_TYPE");
 	_params_handles.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK");
 	_params_handles.fw_min_alt = param_find("VT_FW_MIN_ALT");
+	_params_handles.fw_alt_err = param_find("VT_FW_ALT_ERR");
 	_params_handles.fw_qc_max_pitch = param_find("VT_FW_QC_P");
 	_params_handles.fw_qc_max_roll = param_find("VT_FW_QC_R");
 	_params_handles.front_trans_time_openloop = param_find("VT_F_TR_OL_TM");
@@ -364,6 +365,22 @@ VtolAttitudeControl::vehicle_local_pos_poll()
 
 }
 
+/**
+* Check for setpoint updates.
+*/
+void
+VtolAttitudeControl::vehicle_local_pos_sp_poll()
+{
+	bool updated;
+	/* Check if parameters have changed */
+	orb_check(_local_pos_sp_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_sub, &_local_pos_sp);
+	}
+
+}
+
 /**
 * Check for position setpoint updates.
 */
@@ -571,6 +588,10 @@ VtolAttitudeControl::parameters_update()
 	param_get(_params_handles.fw_min_alt, &v);
 	_params.fw_min_alt = v;
 
+	/* maximum negative altitude error for FW mode (Adaptive QuadChute) */
+	param_get(_params_handles.fw_alt_err, &v);
+	_params.fw_alt_err = v;
+
 	/* maximum pitch angle (QuadChute) */
 	param_get(_params_handles.fw_qc_max_pitch, &l);
 	_params.fw_qc_max_pitch = l;
@@ -666,6 +687,7 @@ void VtolAttitudeControl::task_main()
 	_params_sub            = orb_subscribe(ORB_ID(parameter_update));
 	_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	_local_pos_sub         = orb_subscribe(ORB_ID(vehicle_local_position));
+	_local_pos_sp_sub         = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
 	_pos_sp_triplet_sub    = orb_subscribe(ORB_ID(position_setpoint_triplet));
 	_airspeed_sub          = orb_subscribe(ORB_ID(airspeed));
 	_battery_status_sub	   = orb_subscribe(ORB_ID(battery_status));
@@ -754,6 +776,7 @@ void VtolAttitudeControl::task_main()
 		vehicle_rates_sp_fw_poll();
 		parameters_update_poll();
 		vehicle_local_pos_poll();			// Check for new sensor values
+		vehicle_local_pos_sp_poll();
 		pos_sp_triplet_poll();
 		vehicle_airspeed_poll();
 		vehicle_battery_poll();

src/modules/vtol_att_control/vtol_att_control_main.h

@@ -75,6 +75,7 @@
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vtol_vehicle_status.h>
@@ -114,6 +115,7 @@ public:
 	struct actuator_controls_s 			*get_actuators_mc_in() {return &_actuators_mc_in;}
 	struct actuator_controls_s 			*get_actuators_fw_in() {return &_actuators_fw_in;}
 	struct vehicle_local_position_s 		*get_local_pos() {return &_local_pos;}
+	struct vehicle_local_position_setpoint_s	*get_local_pos_sp() {return &_local_pos_sp;}
 	struct position_setpoint_triplet_s		*get_pos_sp_triplet() {return &_pos_sp_triplet;}
 	struct airspeed_s 				*get_airspeed() {return &_airspeed;}
 	struct battery_status_s 			*get_batt_status() {return &_batt_status;}
@@ -140,6 +142,7 @@ private:
 	int	_params_sub;			//parameter updates subscription
 	int	_manual_control_sp_sub;	//manual control setpoint subscription
 	int	_local_pos_sub;			// sensor subscription
+	int	_local_pos_sp_sub;			// setpoint subscription
 	int	_pos_sp_triplet_sub;			// local position setpoint subscription
 	int	_airspeed_sub;			// airspeed subscription
 	int	_battery_status_sub;	// battery status subscription
@@ -173,6 +176,7 @@ private:
 	struct actuator_controls_s			_actuators_mc_in;	//actuator controls from mc_att_control
 	struct actuator_controls_s			_actuators_fw_in;	//actuator controls from fw_att_control
 	struct vehicle_local_position_s			_local_pos;
+	struct vehicle_local_position_setpoint_s	_local_pos_sp;
 	struct position_setpoint_triplet_s		_pos_sp_triplet;
 	struct airspeed_s 				_airspeed;			// airspeed
 	struct battery_status_s 			_batt_status; 		// battery status
@@ -196,6 +200,7 @@ private:
 		param_t vtol_type;
 		param_t elevons_mc_lock;
 		param_t fw_min_alt;
+		param_t fw_alt_err;
 		param_t fw_qc_max_pitch;
 		param_t fw_qc_max_roll;
 		param_t front_trans_time_openloop;
@@ -227,6 +232,7 @@ private:
 	void 		vehicle_rates_sp_mc_poll();
 	void 		vehicle_rates_sp_fw_poll();
 	void 		vehicle_local_pos_poll();		// Check for changes in sensor values
+	void 		vehicle_local_pos_sp_poll();		// Check for changes in setpoint values
 	void 		pos_sp_triplet_poll();		// Check for changes in position setpoint values
 	void 		vehicle_airspeed_poll();		// Check for changes in airspeed
 	void		vehicle_attitude_setpoint_poll();  //Check for attitude setpoint updates.

src/modules/vtol_att_control/vtol_att_control_params.c

@@ -308,6 +308,16 @@ PARAM_DEFINE_FLOAT(VT_TRANS_MIN_TM, 2.0f);
  */
 PARAM_DEFINE_FLOAT(VT_FW_MIN_ALT, 0.0f);
 
+/**
+ * Adaptive QuadChute
+ *
+ * Maximum negative altitude error, when in fixed wing the altitude drops below this copared to the altitude setpoint
+ * the vehicle will transition back to MC mode and enter failsafe RTL
+ * @min 0.0
+ * @max 200.0
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_FW_ALT_ERR, 0.0f);
 
 /**
  * QuadChute Max Pitch

src/modules/vtol_att_control/vtol_type.cpp

@@ -65,6 +65,7 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) :
 	_actuators_mc_in = _attc->get_actuators_mc_in();
 	_actuators_fw_in = _attc->get_actuators_fw_in();
 	_local_pos = _attc->get_local_pos();
+	_local_pos_sp = _attc->get_local_pos_sp();
 	_airspeed = _attc->get_airspeed();
 	_batt_status = _attc->get_batt_status();
 	_tecs_status = _attc->get_tecs_status();
@@ -220,7 +221,28 @@ void VtolType::check_quadchute_condition()
 		if (_params->fw_min_alt > FLT_EPSILON) {
 
 			if (-(_local_pos->z) < _params->fw_min_alt) {
-				_attc->abort_front_transition("Minimum altitude breached");
+				_attc->abort_front_transition("QuadChute: Minimum altitude breached");
+			}
+		}
+
+		// adaptive quadchute
+		// We use tecs for tracking in FW and local_pos_sp during transitions
+		if (_params->fw_alt_err > FLT_EPSILON) {
+			float altErr = 0.0f;
+
+			if (_tecs_running) {
+				altErr = _tecs_status->altitudeSp - _tecs_status->altitude_filtered;
+
+			} else {
+				altErr = -_local_pos_sp->z - -_local_pos->z;
+
+				if (!_local_pos->z_valid) {
+					altErr = 0.0f;
+				}
+			}
+
+			if (altErr > _params->fw_alt_err) {
+				_attc->abort_front_transition("QuadChute: Altitude error too large");
 			}
 		}
 

src/modules/vtol_att_control/vtol_type.h

@@ -60,6 +60,7 @@ struct Params {
 	int32_t vtol_type;
 	int32_t elevons_mc_lock;		// lock elevons in multicopter mode
 	float fw_min_alt;			// minimum relative altitude for FW mode (QuadChute)
+	float fw_alt_err;			// maximum negative altitude error for FW mode (Adaptive QuadChute)
 	float fw_qc_max_pitch;		// maximum pitch angle FW mode (QuadChute)
 	float fw_qc_max_roll;		// maximum roll angle FW mode (QuadChute)
 	float front_trans_time_openloop;
@@ -168,6 +169,7 @@ protected:
 	struct actuator_controls_s			*_actuators_mc_in;			//actuator controls from mc_att_control
 	struct actuator_controls_s			*_actuators_fw_in;			//actuator controls from fw_att_control
 	struct vehicle_local_position_s			*_local_pos;
+	struct vehicle_local_position_setpoint_s	*_local_pos_sp;
 	struct airspeed_s 				*_airspeed;					// airspeed
 	struct battery_status_s 			*_batt_status; 				// battery status
 	struct tecs_status_s				*_tecs_status;