Disable pusher assist when descending

src/modules/vtol_att_control/standard.cpp

@@ -407,6 +407,12 @@ void Standard::update_mc_state()
 		return;
 	}
 
+	// disable pusher assist when descending
+	if (PX4_ISFINITE(_attc->get_local_pos_sp()->vz) && _attc->get_local_pos_sp()->vz > 0.5f) {
+		return;
+	}
+
+
 	matrix::Dcmf R(matrix::Quatf(_v_att->q));
 	matrix::Dcmf R_sp(matrix::Quatf(_v_att_sp->q_d));
 	matrix::Eulerf euler(R);

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -76,6 +76,7 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	_manual_control_sp_sub(-1),
 	_armed_sub(-1),
 	_local_pos_sub(-1),
+	_local_pos_sp_sub(-1),
 	_airspeed_sub(-1),
 	_battery_status_sub(-1),
 	_vehicle_cmd_sub(-1),
@@ -109,6 +110,7 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	memset(&_actuators_fw_in, 0, sizeof(_actuators_fw_in));
 	memset(&_armed, 0, sizeof(_armed));
 	memset(&_local_pos, 0, sizeof(_local_pos));
+	memset(&_local_pos_sp, 0, sizeof(_local_pos_sp));
 	memset(&_airspeed, 0, sizeof(_airspeed));
 	memset(&_batt_status, 0, sizeof(_batt_status));
 	memset(&_vehicle_cmd, 0, sizeof(_vehicle_cmd));
@@ -373,6 +375,22 @@ VtolAttitudeControl::vehicle_local_pos_poll()
 
 }
 
+/**
+* Check for position 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 mc virtual attitude setpoint updates.
 */
@@ -650,6 +668,7 @@ void VtolAttitudeControl::task_main()
 	_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	_armed_sub             = orb_subscribe(ORB_ID(actuator_armed));
 	_local_pos_sub         = orb_subscribe(ORB_ID(vehicle_local_position));
+	_local_pos_sp_sub         = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
 	_airspeed_sub          = orb_subscribe(ORB_ID(airspeed));
 	_battery_status_sub	   = orb_subscribe(ORB_ID(battery_status));
 	_vehicle_cmd_sub	   = orb_subscribe(ORB_ID(vehicle_command));
@@ -738,6 +757,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();
 		vehicle_airspeed_poll();
 		vehicle_battery_poll();
 		vehicle_cmd_poll();

src/modules/vtol_att_control/vtol_att_control_main.h

@@ -77,6 +77,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/vehicle_rates_setpoint.h>
 #include <uORB/topics/vtol_vehicle_status.h>
 #include <uORB/uORB.h>
@@ -116,6 +117,7 @@ public:
 	struct actuator_controls_s 			*get_actuators_fw_in() {return &_actuators_fw_in;}
 	struct actuator_armed_s 			*get_armed() {return &_armed;}
 	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 airspeed_s 				*get_airspeed() {return &_airspeed;}
 	struct battery_status_s 			*get_batt_status() {return &_batt_status;}
 	struct tecs_status_s 				*get_tecs_status() {return &_tecs_status;}
@@ -142,6 +144,7 @@ private:
 	int	_manual_control_sp_sub;	//manual control setpoint subscription
 	int	_armed_sub;				//arming status subscription
 	int	_local_pos_sub;			// sensor subscription
+	int	_local_pos_sp_sub;			// local position setpoint subscription
 	int	_airspeed_sub;			// airspeed subscription
 	int	_battery_status_sub;	// battery status subscription
 	int	_vehicle_cmd_sub;
@@ -175,6 +178,7 @@ private:
 	struct actuator_controls_s			_actuators_fw_in;	//actuator controls from fw_att_control
 	struct actuator_armed_s				_armed;				//actuator arming status
 	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 vehicle_command_s			_vehicle_cmd;
@@ -226,6 +230,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 position setpoint values
 	void 		vehicle_airspeed_poll();		// Check for changes in airspeed
 	void		vehicle_attitude_setpoint_poll();  //Check for attitude setpoint updates.
 	void		vehicle_attitude_poll();  //Check for attitude updates.