Orbit: Switch to PositionSmoothing library.

This also fixes the bug with altitude not follows and smoothes orbit approach trajectory

src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp

@@ -42,7 +42,7 @@
 
 using namespace matrix;
 
-FlightTaskOrbit::FlightTaskOrbit() : _circle_approach_line(_position)
+FlightTaskOrbit::FlightTaskOrbit()
 {
 	_sticks_data_required = false;
 }
@@ -51,22 +51,28 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)
 {
 	bool ret = true;
 	// save previous velocity and roatation direction
-	float v = fabsf(_v);
-	bool clockwise = _v > 0;
+	bool is_clockwise = _orbit_velocity > 0;
+
+	float new_radius = _orbit_radius;
+	float new_abs_velocity = fabsf(_orbit_velocity);
 
 	// commanded radius
 	if (PX4_ISFINITE(command.param1)) {
-		clockwise = command.param1 > 0;
-		const float r = fabsf(command.param1);
-		ret = ret && setRadius(r);
+		// Note: Radius sign is defined as orbit direction in MAVLINK
+		float radius = fabsf(command.param1);
+		is_clockwise = radius > 0;
+		new_radius = fabsf(radius);
 	}
 
 	// commanded velocity, take sign of radius as rotation direction
 	if (PX4_ISFINITE(command.param2)) {
-		v = command.param2;
+		new_abs_velocity = command.param2;
 	}
 
-	ret = ret && setVelocity(v * (clockwise ? 1.f : -1.f));
+	float new_velocity = (is_clockwise ? 1.f : -1.f) * new_abs_velocity;
+	_sanitizeParams(new_radius, new_velocity);
+	_orbit_radius = new_radius;
+	_orbit_velocity = new_velocity;
 
 	// commanded heading behaviour
 	if (PX4_ISFINITE(command.param3)) {
@@ -91,7 +97,7 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)
 	// commanded altitude
 	if (PX4_ISFINITE(command.param7)) {
 		if (map_projection_initialized(&_global_local_proj_ref)) {
-			_position_setpoint(2) = _global_local_alt0 - command.param7;
+			_center(2) = _global_local_alt0 - command.param7;
 
 		} else {
 			ret = false;
@@ -99,8 +105,11 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)
 	}
 
 	// perpendicularly approach the orbit circle again when new parameters get commanded
-	_in_circle_approach = true;
-	_circle_approach_line.reset();
+	if (!_is_position_on_circle()) {
+		_in_circle_approach = true;
+		_position_smoothing.reset({0.f, 0.f, 0.f}, _velocity, _position);
+		_circle_approach_start_position = _position;
+	}
 
 	return ret;
 }
@@ -108,7 +117,7 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)
 bool FlightTaskOrbit::sendTelemetry()
 {
 	orbit_status_s orbit_status{};
-	orbit_status.radius = math::signNoZero(_v) * _r;
+	orbit_status.radius = math::signNoZero(_orbit_velocity) * _orbit_radius;
 	orbit_status.frame = 0; // MAV_FRAME::MAV_FRAME_GLOBAL
 	orbit_status.yaw_behaviour = _yaw_behaviour;
 
@@ -127,46 +136,29 @@ bool FlightTaskOrbit::sendTelemetry()
 	return true;
 }
 
-bool FlightTaskOrbit::setRadius(float r)
+void FlightTaskOrbit::_sanitizeParams(float &radius, float &velocity) const
 {
 	// clip the radius to be within range
-	r = math::constrain(r, _radius_min, _radius_max);
+	radius = math::constrain(radius, _radius_min, _radius_max);
+	velocity = math::constrain(velocity, -fabsf(_velocity_max), fabsf(_velocity_max));
 
-	// small radius is more important than high velocity for safety
-	if (!checkAcceleration(r, _v, _acceleration_max)) {
-		_v = sign(_v) * sqrtf(_acceleration_max * r);
+	bool exceeds_maximum_acceleration = (velocity * velocity) >= _acceleration_max * radius;
+
+	// value combination is not valid. Reduce velocity instead of
+	// radius, as small radius + low velocity is better for safety
+	if (exceeds_maximum_acceleration) {
+		velocity = sign(velocity) * sqrtf(_acceleration_max * radius);
 	}
-
-	if (fabs(_r - r) > FLT_EPSILON) {
-		_circle_approach_line.reset();
-	}
-
-	_r = r;
-	return true;
 }
 
-bool FlightTaskOrbit::setVelocity(const float v)
-{
-	if (fabs(v) < _velocity_max &&
-	    checkAcceleration(_r, v, _acceleration_max)) {
-		_v = v;
-		return true;
-	}
-
-	return false;
-}
-
-bool FlightTaskOrbit::checkAcceleration(float r, float v, float a)
-{
-	return v * v < a * r;
-}
 
 bool FlightTaskOrbit::activate(const vehicle_local_position_setpoint_s &last_setpoint)
 {
-	bool ret = FlightTaskManualAltitudeSmoothVel::activate(last_setpoint);
-	_r = _radius_min;
-	_v =  1.f;
-	_center = _position.xy();
+	bool ret = FlightTaskManualAltitude::activate(last_setpoint);
+	_orbit_radius = _radius_min;
+	_orbit_velocity =  1.f;
+	_sanitizeParams(_orbit_radius, _orbit_velocity);
+	_center = _position;
 	_initial_heading = _yaw;
 	_slew_rate_yaw.setForcedValue(_yaw);
 	_slew_rate_yaw.setSlewRate(math::radians(_param_mpc_yawrauto_max.get()));
@@ -179,29 +171,60 @@ bool FlightTaskOrbit::activate(const vehicle_local_position_setpoint_s &last_set
 	      && PX4_ISFINITE(_velocity(1))
 	      && PX4_ISFINITE(_velocity(2));
 
+	_position_smoothing.reset({0.f, 0.f, 0.f}, _velocity, _position);
+	_circle_approach_start_position = _position;
+
 	return ret;
 }
 
+
+
 bool FlightTaskOrbit::update()
 {
 	// update altitude
-	bool ret = FlightTaskManualAltitudeSmoothVel::update();
+	bool ret = FlightTaskManualAltitude::update();
+
+	_updateTrajectoryBoundaries();
 
 	// stick input adjusts parameters within a fixed time frame
-	const float r = _r - _sticks.getPositionExpo()(0) * _deltatime * (_radius_max / 8.f);
-	const float v = _v - _sticks.getPositionExpo()(1) * _deltatime * (_velocity_max / 4.f);
+	float radius = _orbit_radius - _sticks.getPositionExpo()(0) * _deltatime * (_radius_max / 8.f);
+	float velocity = _orbit_velocity - _sticks.getPositionExpo()(1) * _deltatime * (_velocity_max / 4.f);
+	_sanitizeParams(radius, velocity);
+	_orbit_radius = radius;
+	_orbit_velocity = velocity;
 
-	setRadius(r);
-	setVelocity(v);
-
-	const Vector2f center_to_position = Vector2f(_position) - _center;
-
-	if (_in_circle_approach) {
-		generate_circle_approach_setpoints(center_to_position);
+	if (_is_position_on_circle()) {
+		if (_in_circle_approach) {
+			_in_circle_approach = false;
+			_altitude_velocity_smoothing.reset(0, _velocity(2), _position(2));
+		}
 
 	} else {
-		generate_circle_setpoints(center_to_position);
-		generate_circle_yaw_setpoints(center_to_position);
+		if (!_in_circle_approach) {
+			_in_circle_approach = true;
+			_position_smoothing.reset({0.f, 0.f, 0.f}, _velocity, _position);
+			_circle_approach_start_position = _position;
+		}
+	}
+
+	if (_in_circle_approach) {
+		_generate_circle_approach_setpoints();
+
+	} else {
+		// this generates x / y setpoints
+		_generate_circle_setpoints();
+		_generate_circle_yaw_setpoints();
+
+		// in case we have a velocity setpoint in altititude (from altitude parent)
+		// smooth this
+		if (!PX4_ISFINITE(_position_setpoint(2))) {
+			_altitude_velocity_smoothing.updateDurations(_velocity_setpoint(2));
+			_altitude_velocity_smoothing.updateTraj(_deltatime);
+			_velocity_setpoint(2) = _altitude_velocity_smoothing.getCurrentVelocity();
+			_acceleration_setpoint(2) = _altitude_velocity_smoothing.getCurrentAcceleration();
+			// set orbit altitude center to expected new altitude
+			_center(2) = _position(2) + _deltatime * _velocity_setpoint(2);
+		}
 	}
 
 	// Apply yaw smoothing
@@ -213,42 +236,121 @@ bool FlightTaskOrbit::update()
 	return ret;
 }
 
-void FlightTaskOrbit::generate_circle_approach_setpoints(const Vector2f &center_to_position)
-{
-	const Vector2f start_to_circle = (_r - center_to_position.norm()) * center_to_position.unit_or_zero();
 
-	if (_circle_approach_line.isEndReached()) {
-		// calculate target point on circle and plan a line trajectory
-		const Vector2f closest_circle_point = Vector2f(_position) + start_to_circle;
-		const Vector3f target = Vector3f(closest_circle_point(0), closest_circle_point(1), _position(2));
-		_circle_approach_line.setLineFromTo(_position, target);
-		_circle_approach_line.setSpeed(_param_mpc_xy_cruise.get());
+void FlightTaskOrbit::_updateTrajectoryBoundaries()
+{
+	// update params of the position smoothing
+	_position_smoothing.setMaxAllowedHorizontalError(_param_mpc_xy_err_max.get());
+	_position_smoothing.setVerticalAcceptanceRadius(_param_nav_mc_alt_rad.get());
+	_position_smoothing.setCruiseSpeed(_param_mpc_xy_cruise.get());
+	_position_smoothing.setHorizontalTrajectoryGain(_param_mpc_xy_traj_p.get());
+	_position_smoothing.setTargetAcceptanceRadius(_horizontal_acceptance_radius);
+
+	// Update the constraints of the trajectories
+	_position_smoothing.setMaxAccelerationXY(_param_mpc_acc_hor.get()); // TODO : Should be computed using heading
+	_position_smoothing.setMaxVelocityXY(_param_mpc_xy_vel_max.get());
+	float max_jerk = _param_mpc_jerk_auto.get();
+	_position_smoothing.setMaxJerk({max_jerk, max_jerk, max_jerk}); // TODO : Should be computed using heading
+	_altitude_velocity_smoothing.setMaxJerk(max_jerk);
+
+	if (_unsmoothed_velocity_setpoint(2) < 0.f) { // up
+		float z_accel_constraint = _param_mpc_acc_up_max.get();
+		float z_vel_constraint = _param_mpc_z_vel_max_up.get();
+
+		_position_smoothing.setMaxVelocityZ(z_vel_constraint);
+		_position_smoothing.setMaxAccelerationZ(z_accel_constraint);
+		_altitude_velocity_smoothing.setMaxVel(z_vel_constraint);
+		_altitude_velocity_smoothing.setMaxAccel(z_accel_constraint);
+
+	} else { // down
+		_position_smoothing.setMaxAccelerationZ(_param_mpc_acc_down_max.get());
+		_position_smoothing.setMaxVelocityZ(_param_mpc_z_vel_max_dn.get());
+		_altitude_velocity_smoothing.setMaxVel(_param_mpc_acc_down_max.get());
+		_altitude_velocity_smoothing.setMaxAccel(_param_mpc_z_vel_max_dn.get());
 	}
 
-	_yaw_setpoint = atan2f(start_to_circle(1), start_to_circle(0));
-
-	// follow the planned line and switch to orbiting once the circle is reached
-	_circle_approach_line.generateSetpoints(_position_setpoint, _velocity_setpoint);
-	_in_circle_approach = !_circle_approach_line.isEndReached();
 }
 
-void FlightTaskOrbit::generate_circle_setpoints(const Vector2f &center_to_position)
+
+bool FlightTaskOrbit::_is_position_on_circle() const
 {
+	return (fabsf(Vector2f(_position - _center).length() - _orbit_radius) < _horizontal_acceptance_radius)
+	       && fabsf(_position(2) - _center(2)) < _param_nav_mc_alt_rad.get();
+
+}
+
+
+bool circle_tangents_for_position(const Vector2f &center, float radius, const Vector2f &position, Vector2f &out1,
+				  Vector2f &out2)
+{
+	const Vector2f d = position - center;
+	const Vector2f dr = {-d(1), d(0)};
+	float d_norm = d.length();
+
+	if (d_norm >= radius) {
+		float rho = radius / d_norm;
+		float ad = rho * rho;
+		float bd = rho * sqrtf(1.f - ad);
+		out1 = center + ad * d + bd * dr;
+		out2 = center + ad * d - bd * dr;
+		return true;
+	}
+
+	return false;
+}
+
+
+void FlightTaskOrbit::_generate_circle_approach_setpoints()
+{
+	const Vector2f center2d = Vector2f(_center);
+	const Vector2f position_to_center_xy = center2d - Vector2f(_position);
+	Vector2f closest_point_on_circle = Vector2f(_position) + position_to_center_xy.unit_or_zero() *
+					   (position_to_center_xy.norm() - _orbit_radius);
+
+	float angle = math::radians(8.f);
+	float s_a = _orbit_velocity >= 0 ? sinf(angle) : -sinf(angle);
+	float c_a = cosf(angle);
+	Vector2f origin_closest = (closest_point_on_circle - center2d);
+	Vector2f target_circle_point_xy =  {
+		center2d(0) + c_a * origin_closest(0) - s_a * origin_closest(1),
+		center2d(1) + s_a * origin_closest(0) + c_a * origin_closest(1)
+	};
+
+	const Vector3f target_circle_point{target_circle_point_xy(0), target_circle_point_xy(1), _center(2)};
+
+	PositionSmoothing::PositionSmoothingSetpoints out_setpoints;
+	_position_smoothing.generateSetpoints(_position, {
+		_circle_approach_start_position, target_circle_point, target_circle_point
+	},
+	{0.f, 0.f, 0.f}, _deltatime, false, out_setpoints);
+
+	_yaw_setpoint = atan2f(position_to_center_xy(1), position_to_center_xy(0));
+
+	_position_setpoint = out_setpoints.position;
+	_velocity_setpoint = out_setpoints.velocity;
+}
+
+void FlightTaskOrbit::_generate_circle_setpoints()
+{
+	Vector3f center_to_position = _position - _center;
 	// xy velocity to go around in a circle
 	Vector2f velocity_xy(-center_to_position(1), center_to_position(0));
 	velocity_xy = velocity_xy.unit_or_zero();
-	velocity_xy *= _v;
+	velocity_xy *= _orbit_velocity;
 
 	// xy velocity adjustment to stay on the radius distance
-	velocity_xy += (_r - center_to_position.norm()) * center_to_position.unit_or_zero();
+	velocity_xy += (_orbit_radius - center_to_position.xy().norm()) * Vector2f(center_to_position).unit_or_zero();
 
 	_position_setpoint(0) = _position_setpoint(1) = NAN;
 	_velocity_setpoint.xy() = velocity_xy;
-	_acceleration_setpoint.xy() = -center_to_position.unit_or_zero() * _v * _v / _r;
+	_acceleration_setpoint.xy() = -Vector2f(center_to_position.unit_or_zero()) * _orbit_velocity * _orbit_velocity /
+				      _orbit_radius;
 }
 
-void FlightTaskOrbit::generate_circle_yaw_setpoints(const Vector2f &center_to_position)
+void FlightTaskOrbit::_generate_circle_yaw_setpoints()
 {
+	Vector3f center_to_position = _position - _center;
+
 	switch (_yaw_behaviour) {
 	case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING:
 		// make vehicle keep the same heading as when the orbit was commanded
@@ -263,8 +365,8 @@ void FlightTaskOrbit::generate_circle_yaw_setpoints(const Vector2f &center_to_po
 		break;
 
 	case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE:
-		_yaw_setpoint = atan2f(sign(_v) * center_to_position(0), -sign(_v) * center_to_position(1));
-		_yawspeed_setpoint = _v / _r;
+		_yaw_setpoint = atan2f(sign(_orbit_velocity) * center_to_position(0), -sign(_orbit_velocity) * center_to_position(1));
+		_yawspeed_setpoint = _orbit_velocity / _orbit_radius;
 		break;
 
 	case orbit_status_s::ORBIT_YAW_BEHAVIOUR_RC_CONTROLLED:
@@ -275,7 +377,7 @@ void FlightTaskOrbit::generate_circle_yaw_setpoints(const Vector2f &center_to_po
 	default:
 		_yaw_setpoint = atan2f(-center_to_position(1), -center_to_position(0));
 		// yawspeed feed-forward because we know the necessary angular rate
-		_yawspeed_setpoint = _v / _r;
+		_yawspeed_setpoint = _orbit_velocity / _orbit_radius;
 		break;
 	}
 }

src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp

@@ -46,10 +46,14 @@
 #include <uORB/topics/orbit_status.h>
 #include "StraightLine.hpp"
 #include <lib/slew_rate/SlewRateYaw.hpp>
+#include <lib/motion_planning/PositionSmoothing.hpp>
+#include <lib/motion_planning/VelocitySmoothing.hpp>
 
-class FlightTaskOrbit : public FlightTaskManualAltitudeSmoothVel
+
+class FlightTaskOrbit : public FlightTaskManualAltitude
 {
 public:
+
 	FlightTaskOrbit();
 	virtual ~FlightTaskOrbit() = default;
 
@@ -57,16 +61,6 @@ public:
 	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
 	bool update() override;
 
-	/**
-	 * Check the feasibility of orbit parameters with respect to
-	 * centripetal acceleration a = v^2 / r
-	 * @param r desired radius
-	 * @param v desired velocity
-	 * @param a maximal allowed acceleration
-	 * @return true on success, false if value not accepted
-	 */
-	bool checkAcceleration(float r, float v, float a);
-
 protected:
 	/**
 	 * Send out telemetry information for the log and MAVLink.
@@ -74,40 +68,61 @@ protected:
 	 */
 	bool sendTelemetry();
 
-	/**
-	 * Change the radius of the circle.
-	 * @param r desired new radius
-	 * @return true on success, false if value not accepted
-	 */
-	bool setRadius(const float r);
-
-	/**
-	 * Change the velocity of the vehicle on the circle.
-	 * @param v desired new velocity
-	 * @return true on success, false if value not accepted
-	 */
-	bool setVelocity(const float v);
-
 private:
-	/** generates setpoints to smoothly reach the closest point on the circle when starting from far away */
-	void generate_circle_approach_setpoints(const matrix::Vector2f &center_to_position);
-	/** generates xy setpoints to make the vehicle orbit */
-	void generate_circle_setpoints(const matrix::Vector2f &center_to_position);
-	/** generates yaw setpoints to control the vehicle's heading */
-	void generate_circle_yaw_setpoints(const matrix::Vector2f &center_to_position);
+	/* TODO: Should be controlled by params */
+	static constexpr float _radius_min = 1.f;
+	static constexpr float _radius_max = 100.f;
+	static constexpr float _velocity_max = 10.f;
+	static constexpr float _acceleration_max = 2.f;
+	static constexpr float _horizontal_acceptance_radius = 1.f;
 
-	float _r = 0.f; /**< radius with which to orbit the target */
-	float _v = 0.f; /**< clockwise tangential velocity for orbiting in m/s */
-	matrix::Vector2f _center; /**< local frame coordinates of the center point */
+	/**
+	 * Check the feasibility of orbit parameters with respect to
+	 * centripetal acceleration a = v^2 / r
+	 * @param radius desired radius
+	 * @param velocity desired velocity
+	 * @param acceleration maximal allowed acceleration
+	 * @return true on success, false if value not accepted
+	 */
+	bool _accelerationValid(float radius, float velocity, float acceleration) const;
+
+	/**
+	 * Checks if desired orbit params are feasible. If not,
+	 * params are modified such that it is possible
+	 * returns a feasible radius.
+	 * @param radius The radius of the orbit. May get modified
+	 * @param velocity The velocity of the orbit. May get modified
+	 * @return Feasible orbit params
+	 */
+	void _sanitizeParams(float &radius, float &velocity) const;
+
+	/**
+	 * @brief updates the trajectory boundaries from props
+	 */
+	void _updateTrajectoryBoundaries();
+
+	/**
+	 * @brief Checks if the current position is on the circle or not
+	 * Uses the params
+	 */
+	bool _is_position_on_circle() const;
+
+	/** generates setpoints to smoothly reach the closest point on the circle when starting from far away */
+	void _generate_circle_approach_setpoints();
+	/** generates xy setpoints to make the vehicle orbit */
+	void _generate_circle_setpoints();
+	/** generates yaw setpoints to control the vehicle's heading */
+	void _generate_circle_yaw_setpoints();
+
+	float _orbit_velocity;
+	float _orbit_radius;
+	matrix::Vector3f _center; /**< local frame coordinates of the center point */
 
 	bool _in_circle_approach = false;
-	StraightLine _circle_approach_line;
-
-	// TODO: create/use parameters for limits
-	const float _radius_min = 1.f;
-	const float _radius_max = 100.f;
-	const float _velocity_max = 10.f;
-	const float _acceleration_max = 2.f;
+	Vector3f _circle_approach_start_position;
+	PositionSmoothing _position_smoothing;
+	VelocitySmoothing _altitude_velocity_smoothing;
+	Vector3f _unsmoothed_velocity_setpoint;
 
 	/** yaw behaviour during the orbit flight according to MAVLink's ORBIT_YAW_BEHAVIOUR enum */
 	int _yaw_behaviour = orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER;
@@ -118,6 +133,14 @@ private:
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise, /**< cruise speed for circle approach */
-		(ParamFloat<px4::params::MPC_YAWRAUTO_MAX>) _param_mpc_yawrauto_max
+		(ParamFloat<px4::params::MPC_YAWRAUTO_MAX>) _param_mpc_yawrauto_max,
+		(ParamFloat<px4::params::MPC_XY_TRAJ_P>) _param_mpc_xy_traj_p,
+		(ParamFloat<px4::params::NAV_MC_ALT_RAD>)
+		_param_nav_mc_alt_rad, //vertical acceptance radius at which waypoints are updated
+		(ParamFloat<px4::params::MPC_XY_ERR_MAX>) _param_mpc_xy_err_max,
+		(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor, // acceleration in flight
+		(ParamFloat<px4::params::MPC_JERK_AUTO>) _param_mpc_jerk_auto,
+		(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
+		(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max
 	)
 };