msg/position_setpoint.msg remove obsolete fields

msg/position_setpoint.msg

@@ -1,45 +1,30 @@
 # this file is only used in the position_setpoint triple as a dependency
 
-uint64 timestamp		# time since system start (microseconds)
+uint64 timestamp                 # time since system start (microseconds)
 
-uint8 SETPOINT_TYPE_POSITION=0	# position setpoint
-uint8 SETPOINT_TYPE_VELOCITY=1	# velocity setpoint
-uint8 SETPOINT_TYPE_LOITER=2	# loiter setpoint
-uint8 SETPOINT_TYPE_TAKEOFF=3	# takeoff setpoint
-uint8 SETPOINT_TYPE_LAND=4	# land setpoint, altitude must be ignored, descend until landing
-uint8 SETPOINT_TYPE_IDLE=5	# do nothing, switch off motors or keep at idle speed (MC)
-uint8 SETPOINT_TYPE_FOLLOW_TARGET=6  # setpoint in NED frame (x, y, z, vx, vy, vz) set by follow target
+uint8 SETPOINT_TYPE_POSITION = 0 # position setpoint
+uint8 SETPOINT_TYPE_LOITER   = 1 # loiter setpoint
+uint8 SETPOINT_TYPE_TAKEOFF  = 2 # takeoff setpoint
+uint8 SETPOINT_TYPE_LAND     = 3 # land setpoint, altitude must be ignored, descend until landing
+uint8 SETPOINT_TYPE_IDLE     = 4 # do nothing, switch off motors or keep at idle speed (MC)
 
-uint8 VELOCITY_FRAME_LOCAL_NED = 1 # MAV_FRAME_LOCAL_NED
-uint8 VELOCITY_FRAME_BODY_NED = 8 # MAV_FRAME_BODY_NED
+uint8 type                       # setpoint type to adjust behavior of position controller
 
-bool valid			# true if setpoint is valid
-uint8 type			# setpoint type to adjust behavior of position controller
+bool valid                       # true if setpoint is valid
 
-float32 vx			# local velocity setpoint in m/s in NED
-float32 vy			# local velocity setpoint in m/s in NED
-float32 vz			# local velocity setpoint in m/s in NED
-bool velocity_valid		# true if local velocity setpoint valid
-uint8 velocity_frame		# to set velocity setpoints in NED or body
-bool alt_valid		# do not set for 3D position control. Set to true if you want z-position control while doing vx,vy velocity control.
+float64 lat                      # latitude, in deg
+float64 lon                      # longitude, in deg
+float32 alt                      # altitude AMSL, in m
 
-float64 lat			# latitude, in deg
-float64 lon			# longitude, in deg
-float32 alt			# altitude AMSL, in m
-float32 yaw			# yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw
-bool yaw_valid			# true if yaw setpoint valid
+float32 yaw                      # yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw
+bool yaw_valid                   # true if yaw setpoint valid
 
-float32 yawspeed		# yawspeed (only for multirotors, in rad/s)
-bool yawspeed_valid		# true if yawspeed setpoint valid
+float32 loiter_radius            # loiter radius (only for fixed wing), in m
+int8 loiter_direction            # loiter direction: 1 = CW, -1 = CCW
 
-int8 landing_gear		# landing gear: see definition of the states in landing_gear.msg
+float32 acceptance_radius        # navigation acceptance_radius if we're doing waypoint navigation
 
-float32 loiter_radius		# loiter radius (only for fixed wing), in m
-int8 loiter_direction		# loiter direction: 1 = CW, -1 = CCW
+float32 cruising_speed           # the generally desired cruising speed (not a hard constraint)
+float32 cruising_throttle        # the generally desired cruising throttle (not a hard constraint)
 
-float32 acceptance_radius   # navigation acceptance_radius if we're doing waypoint navigation
-
-float32 cruising_speed		# the generally desired cruising speed (not a hard constraint)
-float32 cruising_throttle	# the generally desired cruising throttle (not a hard constraint)
-
-bool disable_weather_vane   # VTOL: disable (in auto mode) the weather vane feature that turns the nose into the wind
+bool disable_weather_vane        # VTOL: disable (in auto mode) the weather vane feature that turns the nose into the wind

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

@@ -262,16 +262,13 @@ bool FlightTaskAuto::_evaluateTriplets()
 
 	if (triplet_update || (_current_state != previous_state) || _current_state == State::offtrack) {
 		_updateInternalWaypoints();
-		_mission_gear = _sub_triplet_setpoint.get().current.landing_gear;
 		_yaw_lock = false;
 	}
 
 	if (_param_com_obs_avoid.get()
 	    && _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,
-				_sub_triplet_setpoint.get().next.yawspeed_valid ? _sub_triplet_setpoint.get().next.yawspeed : (float)NAN,
+				_triplet_next_wp, _sub_triplet_setpoint.get().next.yaw, (float)NAN,
 				_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active(), _sub_triplet_setpoint.get().current.type);
 		_obstacle_avoidance.checkAvoidanceProgress(_position, _triplet_prev_wp, _target_acceptance_radius, _closest_pt);
 	}
@@ -290,12 +287,6 @@ bool FlightTaskAuto::_evaluateTriplets()
 			_yawspeed_setpoint = _ext_yaw_handler->get_weathervane_yawrate();
 		}
 
-
-
-	} else if (_type == WaypointType::follow_target && _sub_triplet_setpoint.get().current.yawspeed_valid) {
-		_yawspeed_setpoint = _sub_triplet_setpoint.get().current.yawspeed;
-		_yaw_setpoint = NAN;
-
 	} else {
 		if ((_type != WaypointType::takeoff || _sub_triplet_setpoint.get().current.disable_weather_vane)
 		    && _sub_triplet_setpoint.get().current.yaw_valid) {
@@ -419,23 +410,6 @@ void FlightTaskAuto::_setDefaultConstraints()
 	}
 }
 
-Vector2f FlightTaskAuto::_getTargetVelocityXY()
-{
-	// guard against any bad velocity values
-	const float vx = _sub_triplet_setpoint.get().current.vx;
-	const float vy = _sub_triplet_setpoint.get().current.vy;
-	bool velocity_valid = PX4_ISFINITE(vx) && PX4_ISFINITE(vy) &&
-			      _sub_triplet_setpoint.get().current.velocity_valid;
-
-	if (velocity_valid) {
-		return Vector2f(vx, vy);
-
-	} else {
-		// just return zero speed
-		return Vector2f{};
-	}
-}
-
 State FlightTaskAuto::_getCurrentState()
 {
 	// Calculate the vehicle current state based on the Navigator triplets and the current position.

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

@@ -61,12 +61,10 @@
  */
 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,
-	follow_target = position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET,
 };
 
 enum class State {
@@ -93,7 +91,6 @@ class FlightTaskAuto : public FlightTask
 
 protected:
 	void _setDefaultConstraints() override;
-	matrix::Vector2f _getTargetVelocityXY(); /**< only used for follow-me and only here because of legacy reason.*/
 	void _updateInternalWaypoints(); /**< Depending on state of vehicle, the internal waypoints might differ from target (for instance if offtrack). */
 	bool _compute_heading_from_2D_vector(float &heading, matrix::Vector2f v); /**< Computes and sets heading a 2D vector */
 
@@ -111,7 +108,6 @@ class FlightTaskAuto : public FlightTask
 
 	State _current_state{State::none};
 	float _target_acceptance_radius{0.0f}; /**< Acceptances radius of the target */
-	int _mission_gear{landing_gear_s::GEAR_KEEP};
 
 	float _yaw_sp_prev{NAN};
 	bool _yaw_sp_aligned{false};

src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.cpp

@@ -91,10 +91,6 @@ bool FlightTaskAutoMapper::update()
 		_prepareTakeoffSetpoints();
 		break;
 
-	case WaypointType::velocity:
-		_prepareVelocitySetpoints();
-		break;
-
 	default:
 		_preparePositionSetpoints();
 		break;
@@ -148,15 +144,6 @@ void FlightTaskAutoMapper::_prepareTakeoffSetpoints()
 	_gear.landing_gear = landing_gear_s::GEAR_DOWN;
 }
 
-void FlightTaskAutoMapper::_prepareVelocitySetpoints()
-{
-	// XY Velocity waypoint
-	// TODO : Rewiew that. What is the expected behavior?
-	_position_setpoint = Vector3f(NAN, NAN, _position(2));
-	Vector2f vel_sp_xy = Vector2f(_velocity).unit_or_zero() * _mc_cruise_speed;
-	_velocity_setpoint = Vector3f(vel_sp_xy(0), vel_sp_xy(1), NAN);
-}
-
 void FlightTaskAutoMapper::_preparePositionSetpoints()
 {
 	// Simple waypoint navigation: go to xyz target, with standard limitations

src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.hpp

@@ -57,7 +57,6 @@ class FlightTaskAutoMapper : public FlightTaskAuto
 
 	void _prepareIdleSetpoints();
 	void _prepareLandSetpoints();
-	void _prepareVelocitySetpoints();
 	void _prepareTakeoffSetpoints();
 	void _preparePositionSetpoints();
 

src/modules/follow_target_estimator/TargetEstimator.hpp

@@ -53,11 +53,13 @@
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
 
-static constexpr float GPS_MESSAGE_STALE_TIMEOUT_MS = 3000.0f;  	// Duration after which the connection to the target is considered lost
+static constexpr float GPS_MESSAGE_STALE_TIMEOUT_MS =
+	3000.0f;  	// Duration after which the connection to the target is considered lost
 static constexpr float MINIMUM_TIME_BETWEEN_POS_FUSIONS_MS = 500.0f;
 static constexpr float MINIMUM_TIME_BETWEEN_VEL_FUSIONS_MS = 100.0f;
 static constexpr float ACCELERATION_SATURATION = 20.0f; 		// 2*g
-static constexpr float MINIMUM_SPEED_FOR_TARGET_MOVING = 0.1f; 	// speed threshold above which the target is considered to be moving
+static constexpr float MINIMUM_SPEED_FOR_TARGET_MOVING =
+	0.1f; 	// speed threshold above which the target is considered to be moving
 
 using namespace time_literals;
 

src/modules/navigator/loiter.cpp

@@ -105,7 +105,6 @@ Loiter::set_loiter_position()
 
 	// convert mission item to current setpoint
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-	pos_sp_triplet->current.velocity_valid = false;
 	pos_sp_triplet->previous.valid = false;
 	mission_apply_limitation(_mission_item);
 	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
@@ -130,7 +129,6 @@ Loiter::reposition()
 
 		// convert mission item to current setpoint
 		struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-		pos_sp_triplet->current.velocity_valid = false;
 		pos_sp_triplet->previous.yaw = _navigator->get_local_position()->heading;
 		pos_sp_triplet->previous.lat = _navigator->get_global_position()->lat;
 		pos_sp_triplet->previous.lon = _navigator->get_global_position()->lon;

src/modules/navigator/mission.cpp

@@ -1863,19 +1863,11 @@ bool Mission::position_setpoint_equal(const position_setpoint_s *p1, const posit
 {
 	return ((p1->valid == p2->valid) &&
 		(p1->type == p2->type) &&
-		(fabsf(p1->vx - p2->vx) < FLT_EPSILON) &&
-		(fabsf(p1->vy - p2->vy) < FLT_EPSILON) &&
-		(fabsf(p1->vz - p2->vz) < FLT_EPSILON) &&
-		(p1->velocity_valid == p2->velocity_valid) &&
-		(p1->velocity_frame == p2->velocity_frame) &&
-		(p1->alt_valid == p2->alt_valid) &&
 		(fabs(p1->lat - p2->lat) < DBL_EPSILON) &&
 		(fabs(p1->lon - p2->lon) < DBL_EPSILON) &&
 		(fabsf(p1->alt - p2->alt) < FLT_EPSILON) &&
 		((fabsf(p1->yaw - p2->yaw) < FLT_EPSILON) || (!PX4_ISFINITE(p1->yaw) && !PX4_ISFINITE(p2->yaw))) &&
 		(p1->yaw_valid == p2->yaw_valid) &&
-		(fabsf(p1->yawspeed - p2->yawspeed) < FLT_EPSILON) &&
-		(p1->yawspeed_valid == p2->yawspeed_valid) &&
 		(fabsf(p1->loiter_radius - p2->loiter_radius) < FLT_EPSILON) &&
 		(p1->loiter_direction == p2->loiter_direction) &&
 		(fabsf(p1->acceptance_radius - p2->acceptance_radius) < FLT_EPSILON) &&

src/modules/navigator/navigator_main.cpp

@@ -822,7 +822,6 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data)
 					rep->current.alt = loiter_altitude_amsl;
 					rep->current.valid = true;
 					rep->current.loiter_radius = get_loiter_radius();
-					rep->current.alt_valid = true;
 					rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;
 					rep->current.loiter_direction = 1;
 					rep->current.cruising_throttle = get_cruising_throttle();

src/modules/rover_pos_control/RoverPositionControl.cpp

@@ -349,7 +349,7 @@ RoverPositionControl::control_velocity(const matrix::Vector3f &current_velocity)
 
 		Vector3f desired_body_velocity;
 
-		if (_velocity_frame == VelocityFrame::NED) {
+		if (_velocity_frame == VelocityFrame::BODY) {
 			desired_body_velocity = desired_velocity;
 
 		} else {