FlightTasks: adapt to acceleration based control

src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp

@@ -57,7 +57,7 @@ bool FlightTaskAutoMapper::update()
 	// vehicle exits idle.
 
 	if (_type_previous == WaypointType::idle) {
-		_thrust_setpoint.setNaN();
+		_acceleration_setpoint.setNaN();
 	}
 
 	// during mission and reposition, raise the landing gears but only
@@ -122,7 +122,7 @@ void FlightTaskAutoMapper::_prepareIdleSetpoints()
 	// Send zero thrust setpoint
 	_position_setpoint.setNaN(); // Don't require any position/velocity setpoints
 	_velocity_setpoint.setNaN();
-	_thrust_setpoint.zero();
+	_acceleration_setpoint = Vector3f(0.f, 0.f, 100.f); // High downwards acceleration to make sure there's no thrust
 }
 
 void FlightTaskAutoMapper::_prepareLandSetpoints()

src/lib/flight_tasks/tasks/Descend/FlightTaskDescend.cpp

@@ -40,7 +40,7 @@ bool FlightTaskDescend::activate(vehicle_local_position_setpoint_s last_setpoint
 {
 	bool ret = FlightTask::activate(last_setpoint);
 	// stay level to minimize horizontal drift
-	_thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, NAN);
+	_acceleration_setpoint = matrix::Vector3f(0.f, 0.f, NAN);
 	// keep heading
 	_yaw_setpoint = _yaw;
 	return ret;
@@ -51,12 +51,12 @@ bool FlightTaskDescend::update()
 	if (PX4_ISFINITE(_velocity(2))) {
 		// land with landspeed
 		_velocity_setpoint(2) = _param_mpc_land_speed.get();
-		_thrust_setpoint(2) = NAN;
+		_acceleration_setpoint(2) = NAN;
 
 	} else {
-		// descend with constant thrust (crash landing)
+		// descend with constant acceleration (crash landing)
 		_velocity_setpoint(2) = NAN;
-		_thrust_setpoint(2) = -_param_mpc_thr_hover.get() * 0.7f;
+		_acceleration_setpoint(2) = .3f;
 	}
 
 	return true;

src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.cpp

@@ -41,9 +41,9 @@ bool FlightTaskFailsafe::activate(vehicle_local_position_setpoint_s last_setpoin
 	bool ret = FlightTask::activate(last_setpoint);
 	_position_setpoint = _position;
 	_velocity_setpoint.zero();
-	_thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, -_param_mpc_thr_hover.get() * 0.6f);
+	_acceleration_setpoint = matrix::Vector3f(0.f, 0.f, .3f);
 	_yaw_setpoint = _yaw;
-	_yawspeed_setpoint = 0.0f;
+	_yawspeed_setpoint = 0.f;
 	return ret;
 }
 
@@ -51,27 +51,26 @@ bool FlightTaskFailsafe::update()
 {
 	if (PX4_ISFINITE(_position(0)) && PX4_ISFINITE(_position(1))) {
 		// stay at current position setpoint
-		_velocity_setpoint(0) = _velocity_setpoint(1) = 0.0f;
-		_thrust_setpoint(0) = _thrust_setpoint(1) = 0.0f;
+		_velocity_setpoint(0) = _velocity_setpoint(1) = 0.f;
+		_acceleration_setpoint(0) = _acceleration_setpoint(1) = 0.f;
 
 	} else if (PX4_ISFINITE(_velocity(0)) && PX4_ISFINITE(_velocity(1))) {
 		// don't move along xy
 		_position_setpoint(0) = _position_setpoint(1) = NAN;
-		_thrust_setpoint(0) = _thrust_setpoint(1) = NAN;
+		_acceleration_setpoint(0) = _acceleration_setpoint(1) = NAN;
 	}
 
 	if (PX4_ISFINITE(_position(2))) {
 		// stay at current altitude setpoint
-		_velocity_setpoint(2) = 0.0f;
-		_thrust_setpoint(2) = NAN;
+		_velocity_setpoint(2) = 0.f;
+		_acceleration_setpoint(2) = NAN;
 
 	} else if (PX4_ISFINITE(_velocity(2))) {
 		// land with landspeed
 		_velocity_setpoint(2) = _param_mpc_land_speed.get();
 		_position_setpoint(2) = NAN;
-		_thrust_setpoint(2) = NAN;
+		_acceleration_setpoint(2) = NAN;
 	}
 
 	return true;
-
 }

src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.hpp

@@ -51,8 +51,6 @@ public:
 
 private:
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
-					(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
-					(ParamFloat<px4::params::MPC_THR_HOVER>)
-					_param_mpc_thr_hover /**< throttle value at which vehicle is at hover equilibrium */
+					(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed
 				       )
 };

src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp

@@ -96,10 +96,12 @@ const vehicle_local_position_setpoint_s FlightTask::getPositionSetpoint()
 
 	_acceleration_setpoint.copyTo(vehicle_local_position_setpoint.acceleration);
 	_jerk_setpoint.copyTo(vehicle_local_position_setpoint.jerk);
-	_thrust_setpoint.copyTo(vehicle_local_position_setpoint.thrust);
 	vehicle_local_position_setpoint.yaw = _yaw_setpoint;
 	vehicle_local_position_setpoint.yawspeed = _yawspeed_setpoint;
 
+	// deprecated, only kept for output logging
+	matrix::Vector3f(NAN, NAN, NAN).copyTo(vehicle_local_position_setpoint.thrust);
+
 	return vehicle_local_position_setpoint;
 }
 
@@ -109,7 +111,6 @@ void FlightTask::_resetSetpoints()
 	_velocity_setpoint.setNaN();
 	_acceleration_setpoint.setNaN();
 	_jerk_setpoint.setNaN();
-	_thrust_setpoint.setNaN();
 	_yaw_setpoint = _yawspeed_setpoint = NAN;
 }
 

src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp

@@ -165,7 +165,11 @@ public:
 	virtual void setYawHandler(WeatherVane *ext_yaw_handler) {}
 
 	void updateVelocityControllerIO(const matrix::Vector3f &vel_sp,
-					const matrix::Vector3f &thrust_sp) {_velocity_setpoint_feedback = vel_sp; _thrust_setpoint_feedback = thrust_sp; }
+					const matrix::Vector3f &acc_sp)
+	{
+		_velocity_setpoint_feedback = vel_sp;
+		_acceleration_setpoint_feedback = acc_sp;
+	}
 
 protected:
 	uORB::SubscriptionData<vehicle_local_position_s> _sub_vehicle_local_position{ORB_ID(vehicle_local_position)};
@@ -225,12 +229,10 @@ protected:
 	matrix::Vector3f _velocity_setpoint;
 	matrix::Vector3f _acceleration_setpoint;
 	matrix::Vector3f _jerk_setpoint;
-	matrix::Vector3f _thrust_setpoint;
 	float _yaw_setpoint{};
 	float _yawspeed_setpoint{};
-
 	matrix::Vector3f _velocity_setpoint_feedback;
-	matrix::Vector3f _thrust_setpoint_feedback;
+	matrix::Vector3f _acceleration_setpoint_feedback;
 
 	/* Counters for estimator local position resets */
 	struct {

src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

@@ -38,6 +38,7 @@
 #include "FlightTaskManualAltitude.hpp"
 #include <float.h>
 #include <mathlib/mathlib.h>
+#include <ecl/geo/geo.h>
 
 using namespace matrix;
 
@@ -53,10 +54,10 @@ bool FlightTaskManualAltitude::activate(vehicle_local_position_setpoint_s last_s
 {
 	bool ret = FlightTaskManual::activate(last_setpoint);
 	_yaw_setpoint = NAN;
-	_yawspeed_setpoint = 0.0f;
-	_thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, NAN); // altitude is controlled from position/velocity
+	_yawspeed_setpoint = 0.f;
+	_acceleration_setpoint = Vector3f(0.f, 0.f, NAN); // altitude is controlled from position/velocity
 	_position_setpoint(2) = _position(2);
-	_velocity_setpoint(2) = 0.0f;
+	_velocity_setpoint(2) = 0.f;
 	_setDefaultConstraints();
 
 	_constraints.tilt = math::radians(_param_mpc_man_tilt_max.get());
@@ -348,7 +349,7 @@ void FlightTaskManualAltitude::_updateSetpoints()
 		sp.normalize();
 	}
 
-	_thrust_setpoint.xy() = sp;
+	_acceleration_setpoint.xy() = sp * tanf(math::radians(_param_mpc_man_tilt_max.get())) * CONSTANTS_ONE_G;
 
 	_updateAltitudeLock();
 	_respectGroundSlowdown();

src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp

@@ -165,6 +165,7 @@ void FlightTaskManualPosition::_updateXYlock()
 void FlightTaskManualPosition::_updateSetpoints()
 {
 	FlightTaskManualAltitude::_updateSetpoints(); // needed to get yaw and setpoints in z-direction
+	_acceleration_setpoint.setNaN(); // don't use the horizontal setpoints from FlightTaskAltitude
 
 	_updateXYlock(); // check for position lock
 
@@ -177,8 +178,5 @@ void FlightTaskManualPosition::_updateSetpoints()
 			// vehicle is steady
 			_yawspeed_setpoint += _weathervane_yaw_handler->get_weathervane_yawrate();
 		}
-
 	}
-
-	_thrust_setpoint.setAll(NAN); // don't require any thrust setpoints
 }

src/lib/flight_tasks/tasks/Offboard/FlightTaskOffboard.cpp

@@ -150,7 +150,9 @@ bool FlightTaskOffboard::update()
 
 	// IDLE
 	if (_sub_triplet_setpoint.get().current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
-		_thrust_setpoint.zero();
+		_position_setpoint.setNaN(); // Don't require any position/velocity setpoints
+		_velocity_setpoint.setNaN();
+		_acceleration_setpoint = Vector3f(0.f, 0.f, 100.f); // High downwards acceleration to make sure there's no thrust
 		return true;
 	}
 
@@ -228,9 +230,9 @@ bool FlightTaskOffboard::update()
 	// Acceleration
 	// Note: this is not supported yet and will be mapped to normalized thrust directly.
 	if (_sub_triplet_setpoint.get().current.acceleration_valid) {
-		_thrust_setpoint(0) = _sub_triplet_setpoint.get().current.a_x;
-		_thrust_setpoint(1) = _sub_triplet_setpoint.get().current.a_y;
-		_thrust_setpoint(2) = _sub_triplet_setpoint.get().current.a_z;
+		_acceleration_setpoint(0) = _sub_triplet_setpoint.get().current.a_x;
+		_acceleration_setpoint(1) = _sub_triplet_setpoint.get().current.a_y;
+		_acceleration_setpoint(2) = _sub_triplet_setpoint.get().current.a_z;
 	}
 
 	// use default conditions of upwards position or velocity to take off

src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.cpp

@@ -64,7 +64,7 @@ void FlightTaskTransition::updateAccelerationEstimate()
 {
 	// Estimate the acceleration by filtering the raw derivative of the velocity estimate
 	// This is done to provide a good estimate of the current acceleration to the next flight task after back-transition
-	_acceleration_setpoint = 0.9f * _acceleration_setpoint + 0.1f * (_velocity - _velocity_prev) / _deltatime;
+	_acceleration_setpoint = .9f * _acceleration_setpoint + .1f * (_velocity - _velocity_prev) / _deltatime;
 
 	if (!PX4_ISFINITE(_acceleration_setpoint(0)) ||
 	    !PX4_ISFINITE(_acceleration_setpoint(1)) ||
@@ -79,7 +79,7 @@ bool FlightTaskTransition::update()
 {
 	// level wings during the transition, altitude should be controlled
 	_position_setpoint(2) = _transition_altitude;
-	_thrust_setpoint.xy() = matrix::Vector2f(0.f, 0.f);
+	_acceleration_setpoint.xy() = matrix::Vector2f(0.f, 0.f);
 
 	updateAccelerationEstimate();