FlightTaskAuto: instanciate position_setpoint_triplet only for evaluation

Next step is to only evaluate when there's a topic update
which is a behavioural change that has to carefully be checked.

I have the suspicion the logic assumes certain states to be
reset on every loop iteration by the triplet evaluation.

src/modules/commander/HomePosition.cpp

@@ -310,21 +310,22 @@ void HomePosition::update(bool set_automatically, bool check_if_changed)
 
 	if (_vehicle_gps_position_sub.updated()) {
 		sensor_gps_s vehicle_gps_position;
-		_vehicle_gps_position_sub.copy(&vehicle_gps_position);
 
-		_gps_lat = vehicle_gps_position.latitude_deg;
-		_gps_lon = vehicle_gps_position.longitude_deg;
-		_gps_alt = vehicle_gps_position.altitude_msl_m;
-		_gps_eph = vehicle_gps_position.eph;
-		_gps_epv = vehicle_gps_position.epv;
+		if (_vehicle_gps_position_sub.copy(&vehicle_gps_position)) {
+			_gps_lat = vehicle_gps_position.latitude_deg;
+			_gps_lon = vehicle_gps_position.longitude_deg;
+			_gps_alt = vehicle_gps_position.altitude_msl_m;
+			_gps_eph = vehicle_gps_position.eph;
+			_gps_epv = vehicle_gps_position.epv;
 
-		const hrt_abstime now = hrt_absolute_time();
-		const bool time = (now < vehicle_gps_position.timestamp + 1_s);
-		const bool fix = vehicle_gps_position.fix_type >= kHomePositionGPSRequiredFixType;
-		const bool eph = vehicle_gps_position.eph < kHomePositionGPSRequiredEPH;
-		const bool epv = vehicle_gps_position.epv < kHomePositionGPSRequiredEPV;
-		const bool evh = vehicle_gps_position.s_variance_m_s < kHomePositionGPSRequiredEVH;
-		_gps_position_for_home_valid = time && fix && eph && epv && evh;
+			const hrt_abstime now = hrt_absolute_time();
+			const bool time = (now < vehicle_gps_position.timestamp + 1_s);
+			const bool fix = vehicle_gps_position.fix_type >= kHomePositionGPSRequiredFixType;
+			const bool eph = vehicle_gps_position.eph < kHomePositionGPSRequiredEPH;
+			const bool epv = vehicle_gps_position.epv < kHomePositionGPSRequiredEPV;
+			const bool evh = vehicle_gps_position.s_variance_m_s < kHomePositionGPSRequiredEVH;
+			_gps_position_for_home_valid = time && fix && eph && epv && evh;
+		}
 	}
 
 	const vehicle_local_position_s &lpos = _local_position_sub.get();

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

@@ -92,10 +92,12 @@ bool FlightTaskAuto::updateInitialize()
 
 	_sub_home_position.update();
 	_sub_vehicle_status.update();
-	_sub_triplet_setpoint.update();
+
+	position_setpoint_triplet_s triplet;
+	ret = ret && _position_setpoint_triplet_sub.copy(&triplet) && _evaluatePositionSetpointTriplet(triplet);
 
 	// require valid reference and valid target
-	ret = ret && _evaluateGlobalReference() && _evaluateTriplets();
+	ret = ret && _evaluateGlobalReference();
 	// require valid position
 	ret = ret && _position.isAllFinite() && _velocity.isAllFinite();
 
@@ -111,7 +113,7 @@ bool FlightTaskAuto::update()
 	// 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) {
+	if (_type_previous == position_setpoint_s::SETPOINT_TYPE_IDLE) {
 		_acceleration_setpoint.setNaN();
 	}
 
@@ -122,18 +124,18 @@ bool FlightTaskAuto::update()
 	}
 
 	switch (_type) {
-	case WaypointType::idle:
+	case position_setpoint_s::SETPOINT_TYPE_IDLE:
 		// Send zero thrust setpoint
 		_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
 		break;
 
-	case WaypointType::land:
+	case position_setpoint_s::SETPOINT_TYPE_LAND:
 		_prepareLandSetpoints();
 		break;
 
-	case WaypointType::velocity:
+	case position_setpoint_s::SETPOINT_TYPE_VELOCITY:
 		// XY Velocity waypoint
 		// TODO : Rewiew that. What is the expected behavior?
 		_position_setpoint = Vector3f(NAN, NAN, _position(2));
@@ -141,14 +143,14 @@ bool FlightTaskAuto::update()
 		_velocity_setpoint(2) = NAN;
 		break;
 
-	case WaypointType::loiter:
+	case position_setpoint_s::SETPOINT_TYPE_LOITER:
 		if (_param_mpc_land_rc_help.get() && _sticks.checkAndUpdateStickInputs()) {
 			rcHelpModifyYaw(_yaw_setpoint);
 		}
 
 	// FALLTHROUGH
-	case WaypointType::takeoff:
-	case WaypointType::position:
+	case position_setpoint_s::SETPOINT_TYPE_TAKEOFF:
+	case position_setpoint_s::SETPOINT_TYPE_POSITION:
 	default:
 		// Simple waypoint navigation: go to xyz target, with standard limitations
 		_position_setpoint = _target;
@@ -244,7 +246,7 @@ void FlightTaskAuto::_prepareLandSetpoints()
 		vertical_speed = _param_mpc_land_crawl_speed.get();
 	}
 
-	if (_type_previous != WaypointType::land) {
+	if (_type_previous != position_setpoint_s::SETPOINT_TYPE_LAND) {
 		// initialize yaw and xy-position
 		_land_heading = _yaw_setpoint;
 		_stick_acceleration_xy.resetPosition(Vector2f(_target(0), _target(1)));
@@ -342,7 +344,7 @@ void FlightTaskAuto::_limitYawRate()
 	}
 }
 
-bool FlightTaskAuto::_evaluateTriplets()
+bool FlightTaskAuto::_evaluatePositionSetpointTriplet(const position_setpoint_triplet_s &triplet)
 {
 	// TODO: fix the issues mentioned below
 	// We add here some conditions that are only required because:
@@ -358,25 +360,23 @@ bool FlightTaskAuto::_evaluateTriplets()
 
 	// Check if triplet is valid. There must be at least a valid altitude.
 
-	if (!_sub_triplet_setpoint.get().current.valid || !PX4_ISFINITE(_sub_triplet_setpoint.get().current.alt)) {
+	if (!triplet.current.valid || !PX4_ISFINITE(triplet.current.alt)) {
 		// Best we can do is to just set all waypoints to current state
 		_prev_prev_wp = _triplet_prev_wp = _triplet_target = _triplet_next_wp = _position;
-		_type = WaypointType::loiter;
+		_type = position_setpoint_s::SETPOINT_TYPE_LOITER;
 		_yaw_setpoint = _yaw;
 		_yawspeed_setpoint = NAN;
-		_target_acceptance_radius = _sub_triplet_setpoint.get().current.acceptance_radius;
+		_target_acceptance_radius = triplet.current.acceptance_radius;
 		_updateInternalWaypoints();
 		return true;
 	}
 
-	_type = (WaypointType)_sub_triplet_setpoint.get().current.type;
+	_type = triplet.current.type;
 
 	// Prioritize cruise speed from the triplet when it's valid and more recent than the previously commanded cruise speed
-	const float cruise_speed_from_triplet = _sub_triplet_setpoint.get().current.cruising_speed;
-
-	if (PX4_ISFINITE(cruise_speed_from_triplet)
-	    && (_sub_triplet_setpoint.get().current.timestamp > _time_last_cruise_speed_override)) {
-		_mc_cruise_speed = cruise_speed_from_triplet;
+	if (PX4_ISFINITE(triplet.current.cruising_speed)
+	    && (triplet.current.timestamp > _time_last_cruise_speed_override)) {
+		_mc_cruise_speed = triplet.current.cruising_speed;
 	}
 
 	if (!PX4_ISFINITE(_mc_cruise_speed) || (_mc_cruise_speed < FLT_EPSILON)) {
@@ -390,8 +390,8 @@ bool FlightTaskAuto::_evaluateTriplets()
 	// Temporary target variable where we save the local reprojection of the latest navigator current triplet.
 	Vector3f tmp_target;
 
-	if (!PX4_ISFINITE(_sub_triplet_setpoint.get().current.lat)
-	    || !PX4_ISFINITE(_sub_triplet_setpoint.get().current.lon)) {
+	if (!PX4_ISFINITE(triplet.current.lat)
+	    || !PX4_ISFINITE(triplet.current.lon)) {
 		// No position provided in xy. Lock position
 		if (!_lock_position_xy.isAllFinite()) {
 			tmp_target(0) = _lock_position_xy(0) = _position(0);
@@ -407,31 +407,29 @@ bool FlightTaskAuto::_evaluateTriplets()
 		_lock_position_xy.setAll(NAN);
 
 		// Convert from global to local frame.
-		_reference_position.project(_sub_triplet_setpoint.get().current.lat, _sub_triplet_setpoint.get().current.lon,
+		_reference_position.project(triplet.current.lat, triplet.current.lon,
 					    tmp_target(0), tmp_target(1));
 	}
 
-	tmp_target(2) = -(_sub_triplet_setpoint.get().current.alt - _reference_altitude);
+	tmp_target(2) = -(triplet.current.alt - _reference_altitude);
 
 	// Check if anything has changed. We do that by comparing the temporary target
 	// to the internal _triplet_target.
 	// TODO This is a hack and it would be much better if the navigator only sends out a waypoints once they have changed.
 
 	bool triplet_update = true;
-	const bool prev_next_validity_changed = (_prev_was_valid != _sub_triplet_setpoint.get().previous.valid)
-						|| (_next_was_valid != _sub_triplet_setpoint.get().next.valid);
+	const bool prev_next_validity_changed = (_prev_was_valid != triplet.previous.valid)
+						|| (_next_was_valid != triplet.next.valid);
 
 	if (_triplet_target.isAllFinite()
-	    && fabsf(_triplet_target(0) - tmp_target(0)) < 0.001f
-	    && fabsf(_triplet_target(1) - tmp_target(1)) < 0.001f
-	    && fabsf(_triplet_target(2) - tmp_target(2)) < 0.001f
+	    && (_triplet_target == tmp_target)
 	    && !prev_next_validity_changed) {
 		// Nothing has changed: just keep old waypoints.
 		triplet_update = false;
 
 	} else {
 		_triplet_target = tmp_target;
-		_target_acceptance_radius = _sub_triplet_setpoint.get().current.acceptance_radius;
+		_target_acceptance_radius = triplet.current.acceptance_radius;
 
 		if (!Vector2f(_triplet_target).isAllFinite()) {
 			// Horizontal target is not finite.
@@ -446,34 +444,34 @@ bool FlightTaskAuto::_evaluateTriplets()
 		// If _triplet_target has updated, update also _triplet_prev_wp and _triplet_next_wp.
 		_prev_prev_wp = _triplet_prev_wp;
 
-		if (_isFinite(_sub_triplet_setpoint.get().previous) && _sub_triplet_setpoint.get().previous.valid) {
-			_reference_position.project(_sub_triplet_setpoint.get().previous.lat,
-						    _sub_triplet_setpoint.get().previous.lon, _triplet_prev_wp(0), _triplet_prev_wp(1));
-			_triplet_prev_wp(2) = -(_sub_triplet_setpoint.get().previous.alt - _reference_altitude);
+		if (_isFinite(triplet.previous) && triplet.previous.valid) {
+			_reference_position.project(triplet.previous.lat,
+						    triplet.previous.lon, _triplet_prev_wp(0), _triplet_prev_wp(1));
+			_triplet_prev_wp(2) = -(triplet.previous.alt - _reference_altitude);
 
 		} else {
 			_triplet_prev_wp = _triplet_target;
 		}
 
-		_prev_was_valid = _sub_triplet_setpoint.get().previous.valid;
+		_prev_was_valid = triplet.previous.valid;
 
-		if (_type == WaypointType::loiter) {
+		if (_type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
 			_triplet_next_wp = _triplet_target;
 
-		} else if (_isFinite(_sub_triplet_setpoint.get().next) && _sub_triplet_setpoint.get().next.valid) {
-			_reference_position.project(_sub_triplet_setpoint.get().next.lat,
-						    _sub_triplet_setpoint.get().next.lon, _triplet_next_wp(0), _triplet_next_wp(1));
-			_triplet_next_wp(2) = -(_sub_triplet_setpoint.get().next.alt - _reference_altitude);
+		} else if (_isFinite(triplet.next) && triplet.next.valid) {
+			_reference_position.project(triplet.next.lat,
+						    triplet.next.lon, _triplet_next_wp(0), _triplet_next_wp(1));
+			_triplet_next_wp(2) = -(triplet.next.alt - _reference_altitude);
 
 		} else {
 			_triplet_next_wp = _triplet_target;
 		}
 
-		_next_was_valid = _sub_triplet_setpoint.get().next.valid;
+		_next_was_valid = triplet.next.valid;
 	}
 
 	// activation/deactivation of weather vane is based on parameter WV_EN and setting of navigator (allow_weather_vane)
-	_weathervane.setNavigatorForceDisabled(PX4_ISFINITE(_sub_triplet_setpoint.get().current.yaw));
+	_weathervane.setNavigatorForceDisabled(PX4_ISFINITE(triplet.current.yaw));
 
 	// Calculate the current vehicle state and check if it has updated.
 	State previous_state = _current_state;
@@ -487,9 +485,9 @@ bool FlightTaskAuto::_evaluateTriplets()
 	    && _sub_vehicle_status.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
 		_obstacle_avoidance.updateAvoidanceDesiredWaypoints(_triplet_target, _yaw_setpoint, _yawspeed_setpoint,
 				_triplet_next_wp,
-				_sub_triplet_setpoint.get().next.yaw,
+				triplet.next.yaw,
 				(float)NAN,
-				_weathervane.isActive(), _sub_triplet_setpoint.get().current.type);
+				_weathervane.isActive(), triplet.current.type);
 		_obstacle_avoidance.checkAvoidanceProgress(
 			_position, _triplet_prev_wp, _target_acceptance_radius, Vector2f(_closest_pt));
 	}
@@ -516,8 +514,8 @@ bool FlightTaskAuto::_evaluateTriplets()
 			_yaw_setpoint = NAN;
 			_yawspeed_setpoint = 0.f;
 
-		} else if (PX4_ISFINITE(_sub_triplet_setpoint.get().current.yaw)) {
-			_yaw_setpoint = _sub_triplet_setpoint.get().current.yaw;
+		} else if (PX4_ISFINITE(triplet.current.yaw)) {
+			_yaw_setpoint = triplet.current.yaw;
 			_yawspeed_setpoint = NAN;
 
 		} else {
@@ -830,7 +828,7 @@ void FlightTaskAuto::_updateTrajConstraints()
 		// until the constraints don't do things like cause controller integrators to saturate. Once the controller
 		// doesn't use z speed constraints, this can go in _prepareTakeoffSetpoints(). Accel limit is to
 		// emulate the motor ramp (also done in the controller) so that the controller can actually track the setpoint.
-		if (_type == WaypointType::takeoff &&  _dist_to_ground < _param_mpc_land_alt1.get()) {
+		if (_type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&  _dist_to_ground < _param_mpc_land_alt1.get()) {
 			z_vel_constraint = _param_mpc_tko_speed.get();
 			z_accel_constraint = math::min(z_accel_constraint, _param_mpc_tko_speed.get() / _param_mpc_tko_ramp_t.get());
 

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

@@ -60,20 +60,6 @@
 #include <lib/avoidance/ObstacleAvoidance.hpp>
 #endif
 
-/**
- * This enum has to agree with position_setpoint_s type definition
- * The only reason for not using the struct position_setpoint is because
- * of the size
- */
-enum class WaypointType : int {
-	position = position_setpoint_s::SETPOINT_TYPE_POSITION,
-	velocity = position_setpoint_s::SETPOINT_TYPE_VELOCITY,
-	loiter = position_setpoint_s::SETPOINT_TYPE_LOITER,
-	takeoff = position_setpoint_s::SETPOINT_TYPE_TAKEOFF,
-	land = position_setpoint_s::SETPOINT_TYPE_LAND,
-	idle = position_setpoint_s::SETPOINT_TYPE_IDLE
-};
-
 enum class State {
 	offtrack, /**< Vehicle is more than cruise speed away from track */
 	target_behind, /**< Vehicle is in front of target. */
@@ -135,13 +121,12 @@ protected:
 	matrix::Vector3f _next_wp{}; /**< The next waypoint after target (local frame). If no next setpoint is available, next is set to target. */
 	bool _next_was_valid{false};
 	float _mc_cruise_speed{NAN}; /**< Requested cruise speed. If not valid, default cruise speed is used. */
-	WaypointType _type{WaypointType::idle}; /**< Type of current target triplet. */
 
 	uORB::SubscriptionData<home_position_s>			_sub_home_position{ORB_ID(home_position)};
 	uORB::SubscriptionData<vehicle_status_s>		_sub_vehicle_status{ORB_ID(vehicle_status)};
 
 	State _current_state{State::none};
-	float _target_acceptance_radius{0.0f}; /**< Acceptances radius of the target */
+	float _target_acceptance_radius{0.f}; /**< Acceptances radius of the target */
 
 	float _yaw_sp_prev{NAN};
 	AlphaFilter<float> _yawspeed_filter;
@@ -156,7 +141,6 @@ protected:
 	StickYaw _stick_yaw{this};
 	matrix::Vector3f _land_position;
 	float _land_heading;
-	WaypointType _type_previous{WaypointType::idle}; /**< Previous type of current target triplet. */
 	bool _is_emergency_braking_active{false};
 	bool _want_takeoff{false};
 
@@ -195,8 +179,9 @@ private:
 	matrix::Vector2f _lock_position_xy{NAN, NAN}; /**< if no valid triplet is received, lock positition to current position */
 	bool _yaw_lock{false}; /**< if within acceptance radius, lock yaw to current yaw */
 
-	uORB::SubscriptionData<position_setpoint_triplet_s> _sub_triplet_setpoint{ORB_ID(position_setpoint_triplet)};
-
+	uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)};
+	uint8_t _type{position_setpoint_s::SETPOINT_TYPE_IDLE}; ///< Type of current target triplet
+	uint8_t _type_previous{position_setpoint_s::SETPOINT_TYPE_IDLE}; ///< Previous type of current target triplet
 	matrix::Vector3f
 	_triplet_target; /**< current triplet from navigator which may differ from the intenal one (_target) depending on the vehicle state. */
 	matrix::Vector3f
@@ -216,7 +201,7 @@ private:
 	matrix::Vector3f _initial_land_position;
 
 	void _limitYawRate(); /**< Limits the rate of change of the yaw setpoint. */
-	bool _evaluateTriplets(); /**< Checks and sets triplets. */
+	bool _evaluatePositionSetpointTriplet(const position_setpoint_triplet_s &triplet);
 	bool _isFinite(const position_setpoint_s &sp); /**< Checks if all waypoint triplets are finite. */
 	bool _evaluateGlobalReference(); /**< Check is global reference is available. */
 	State _getCurrentState(); /**< Computes the current vehicle state based on the vehicle position and navigator triplets. */