Acceleration based control/feed-forward

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -164,9 +164,6 @@ then
 	param set MPC_XY_VEL_I 0.02
 	param set MPC_XY_VEL_D 0.016
 
-	param set MPC_JERK_MIN 10
-	param set MPC_JERK_MAX 20
-	param set MPC_ACC_HOR_MAX 3
 	param set MPC_SPOOLUP_TIME 0.5
 	param set MPC_TKO_RAMP_T 1
 

ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark

@@ -71,8 +71,7 @@ then
 	param set MPC_ACC_HOR_MAX	2
 	param set MPC_ACC_UP_MAX	3
 	param set MC_AIRMODE 1
-	param set MPC_JERK_AUTO	8
-	param set MPC_JERK_AUTO	15
+	param set MPC_JERK_AUTO	4
 	param set MPC_LAND_SPEED 1
 	param set MPC_MAN_TILT_MAX 25
 	param set MPC_MAN_Y_MAX 40

ROMFS/px4fmu_common/init.d/rc.vtol_defaults

@@ -9,7 +9,6 @@ set VEHICLE_TYPE vtol
 
 if [ $AUTOCNF = yes ]
 then
-
 	# to minimize cpu usage on older boards limit inner loop to 400 Hz
 	param set IMU_GYRO_RATEMAX 400
 
@@ -23,6 +22,7 @@ then
 	param set MPC_XY_CRUISE 3
 	param set MPC_XY_VEL_MAX 4
 	param set MPC_Z_VEL_MAX_DN 1.5
+	param set MPC_JERK_MAX 4.5
 
 	param set NAV_ACC_RAD 3
 

src/lib/flight_tasks/CMakeLists.txt

@@ -57,7 +57,6 @@ list(APPEND flight_tasks_all
 	ManualPosition
 	ManualPositionSmooth
 	ManualPositionSmoothVel
-	Sport
 	AutoLineSmoothVel
 	AutoFollowMe
 	Offboard

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,37 +41,36 @@ 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;
 }
 
 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 @@ class FlightTaskFailsafe : public FlightTask
 
 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 @@ class FlightTask : public ModuleParams
 	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 @@ class FlightTask : public ModuleParams
 	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