FlightTaskAuto: merge activate(), reActivate() and update() from FlightTaskAutoLineSmoothVel

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp

@@ -53,14 +53,49 @@ FlightTaskAuto::FlightTaskAuto() :
 bool FlightTaskAuto::activate(const vehicle_local_position_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTask::activate(last_setpoint);
+
 	_position_setpoint = _position;
 	_velocity_setpoint = _velocity;
 	_yaw_setpoint = _yaw;
 	_yawspeed_setpoint = 0.0f;
 	_setDefaultConstraints();
+
+	// Set setpoints equal current state.
+	_velocity_setpoint = _velocity;
+	_position_setpoint = _position;
+
+	Vector3f vel_prev{last_setpoint.vx, last_setpoint.vy, last_setpoint.vz};
+	Vector3f pos_prev{last_setpoint.x, last_setpoint.y, last_setpoint.z};
+	Vector3f accel_prev{last_setpoint.acceleration};
+
+	for (int i = 0; i < 3; i++) {
+		// If the position setpoint is unknown, set to the current postion
+		if (!PX4_ISFINITE(pos_prev(i))) { pos_prev(i) = _position(i); }
+
+		// If the velocity setpoint is unknown, set to the current velocity
+		if (!PX4_ISFINITE(vel_prev(i))) { vel_prev(i) = _velocity(i); }
+
+		// No acceleration estimate available, set to zero if the setpoint is NAN
+		if (!PX4_ISFINITE(accel_prev(i))) { accel_prev(i) = 0.f; }
+	}
+
+	_position_smoothing.reset(accel_prev, vel_prev, pos_prev);
+
+	_yaw_sp_prev = PX4_ISFINITE(last_setpoint.yaw) ? last_setpoint.yaw : _yaw;
+	_updateTrajConstraints();
+	_is_emergency_braking_active = false;
+
 	return ret;
 }
 
+void FlightTaskAuto::reActivate()
+{
+	FlightTask::reActivate();
+
+	// On ground, reset acceleration and velocity to zero
+	_position_smoothing.reset({0.f, 0.f, 0.f}, {0.f, 0.f, 0.7f}, _position);
+}
+
 bool FlightTaskAuto::updateInitialize()
 {
 	bool ret = FlightTask::updateInitialize();
@@ -82,6 +117,70 @@ bool FlightTaskAuto::updateInitialize()
 	return ret;
 }
 
+bool FlightTaskAuto::update()
+{
+	bool ret = FlightTask::update();
+	// always reset constraints because they might change depending on the type
+	_setDefaultConstraints();
+
+	// The only time a thrust set-point is sent out is during
+	// idle. Hence, reset thrust set-point to NAN in case the
+	// vehicle exits idle.
+
+	if (_type_previous == WaypointType::idle) {
+		_acceleration_setpoint.setNaN();
+	}
+
+	// during mission and reposition, raise the landing gears but only
+	// if altitude is high enough
+	if (_highEnoughForLandingGear()) {
+		_gear.landing_gear = landing_gear_s::GEAR_UP;
+	}
+
+	switch (_type) {
+	case WaypointType::idle:
+		_prepareIdleSetpoints();
+		break;
+
+	case WaypointType::land:
+		_prepareLandSetpoints();
+		break;
+
+	case WaypointType::loiter:
+
+	/* fallthrought */
+	case WaypointType::position:
+		_preparePositionSetpoints();
+		break;
+
+	case WaypointType::takeoff:
+		_prepareTakeoffSetpoints();
+		break;
+
+	case WaypointType::velocity:
+		_prepareVelocitySetpoints();
+		break;
+
+	default:
+		_preparePositionSetpoints();
+		break;
+	}
+
+	if (_param_com_obs_avoid.get()) {
+		_obstacle_avoidance.updateAvoidanceDesiredSetpoints(_position_setpoint, _velocity_setpoint, (int)_type);
+		_obstacle_avoidance.injectAvoidanceSetpoints(_position_setpoint, _velocity_setpoint, _yaw_setpoint,
+				_yawspeed_setpoint);
+	}
+
+
+	_generateSetpoints();
+
+	// update previous type
+	_type_previous = _type;
+
+	return ret;
+}
+
 bool FlightTaskAuto::updateFinalize()
 {
 	// All the auto FlightTasks have to comply with defined maximum yaw rate

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp

@@ -88,7 +88,9 @@ public:
 
 	virtual ~FlightTaskAuto() = default;
 	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
+	void reActivate() override;
 	bool updateInitialize() override;
+	bool update() override;
 	bool updateFinalize() override;
 
 	/**

src/modules/flight_mode_manager/tasks/AutoFollowMe/FlightTaskAutoFollowMe.cpp

@@ -42,9 +42,8 @@ using namespace matrix;
 
 bool FlightTaskAutoFollowMe::update()
 {
-	bool ret = FlightTaskAuto::update();
 	_position_setpoint = _target;
 	matrix::Vector2f vel_sp = _getTargetVelocityXY();
 	_velocity_setpoint = matrix::Vector3f(vel_sp(0), vel_sp(1), 0.0f);
-	return ret;
+	return true;
 }

src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp

@@ -39,107 +39,3 @@
 #include <mathlib/mathlib.h>
 
 using namespace matrix;
-
-bool FlightTaskAutoLineSmoothVel::activate(const vehicle_local_position_setpoint_s &last_setpoint)
-{
-	bool ret = FlightTaskAuto::activate(last_setpoint);
-
-	// Set setpoints equal current state.
-	_velocity_setpoint = _velocity;
-	_position_setpoint = _position;
-
-	Vector3f vel_prev{last_setpoint.vx, last_setpoint.vy, last_setpoint.vz};
-	Vector3f pos_prev{last_setpoint.x, last_setpoint.y, last_setpoint.z};
-	Vector3f accel_prev{last_setpoint.acceleration};
-
-	for (int i = 0; i < 3; i++) {
-		// If the position setpoint is unknown, set to the current postion
-		if (!PX4_ISFINITE(pos_prev(i))) { pos_prev(i) = _position(i); }
-
-		// If the velocity setpoint is unknown, set to the current velocity
-		if (!PX4_ISFINITE(vel_prev(i))) { vel_prev(i) = _velocity(i); }
-
-		// No acceleration estimate available, set to zero if the setpoint is NAN
-		if (!PX4_ISFINITE(accel_prev(i))) { accel_prev(i) = 0.f; }
-	}
-
-	_position_smoothing.reset(accel_prev, vel_prev, pos_prev);
-
-	_yaw_sp_prev = PX4_ISFINITE(last_setpoint.yaw) ? last_setpoint.yaw : _yaw;
-	_updateTrajConstraints();
-	_is_emergency_braking_active = false;
-
-	return ret;
-}
-
-void FlightTaskAutoLineSmoothVel::reActivate()
-{
-	FlightTaskAuto::reActivate();
-
-	// On ground, reset acceleration and velocity to zero
-	_position_smoothing.reset({0.f, 0.f, 0.f}, {0.f, 0.f, 0.7f}, _position);
-}
-
-bool FlightTaskAutoLineSmoothVel::update()
-{
-	bool ret = FlightTaskAuto::update();
-	// always reset constraints because they might change depending on the type
-	_setDefaultConstraints();
-
-	// The only time a thrust set-point is sent out is during
-	// idle. Hence, reset thrust set-point to NAN in case the
-	// vehicle exits idle.
-
-	if (_type_previous == WaypointType::idle) {
-		_acceleration_setpoint.setNaN();
-	}
-
-	// during mission and reposition, raise the landing gears but only
-	// if altitude is high enough
-	if (_highEnoughForLandingGear()) {
-		_gear.landing_gear = landing_gear_s::GEAR_UP;
-	}
-
-	switch (_type) {
-	case WaypointType::idle:
-		_prepareIdleSetpoints();
-		break;
-
-	case WaypointType::land:
-		_prepareLandSetpoints();
-		break;
-
-	case WaypointType::loiter:
-
-	/* fallthrought */
-	case WaypointType::position:
-		_preparePositionSetpoints();
-		break;
-
-	case WaypointType::takeoff:
-		_prepareTakeoffSetpoints();
-		break;
-
-	case WaypointType::velocity:
-		_prepareVelocitySetpoints();
-		break;
-
-	default:
-		_preparePositionSetpoints();
-		break;
-	}
-
-	if (_param_com_obs_avoid.get()) {
-		_obstacle_avoidance.updateAvoidanceDesiredSetpoints(_position_setpoint, _velocity_setpoint, (int)_type);
-		_obstacle_avoidance.injectAvoidanceSetpoints(_position_setpoint, _velocity_setpoint, _yaw_setpoint,
-				_yawspeed_setpoint);
-	}
-
-
-	_generateSetpoints();
-
-	// update previous type
-	_type_previous = _type;
-
-	return ret;
-}

src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp

@@ -47,8 +47,4 @@ class FlightTaskAutoLineSmoothVel : public FlightTaskAuto
 public:
 	FlightTaskAutoLineSmoothVel() = default;
 	virtual ~FlightTaskAutoLineSmoothVel() = default;
-
-	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
-	void reActivate() override;
-	bool update() override;
 };