update UUV and rover controllers to use trajectory_setpoint and cleanup unused position_setpoint fields

msg/position_setpoint.msg

@@ -16,11 +16,6 @@ uint8 VELOCITY_FRAME_BODY_NED = 8 # MAV_FRAME_BODY_NED
 bool valid			# true if setpoint is valid
 uint8 type			# setpoint type to adjust behavior of position controller
 
-float32 x			# local position setpoint in m in NED
-float32 y			# local position setpoint in m in NED
-float32 z			# local position setpoint in m in NED
-bool position_valid		# true if local position setpoint 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

src/modules/navigator/follow_target.cpp

@@ -327,7 +327,6 @@ void FollowTarget::update_position_sp(bool use_velocity, bool use_position, floa
 	mission_apply_limitation(_mission_item);
 	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 	pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET;
-	pos_sp_triplet->current.position_valid = use_position;
 	pos_sp_triplet->current.velocity_valid = use_velocity;
 	pos_sp_triplet->current.vx = _current_vel(0);
 	pos_sp_triplet->current.vy = _current_vel(1);

src/modules/navigator/mission.cpp

@@ -1838,10 +1838,6 @@ bool Mission::position_setpoint_equal(const position_setpoint_s *p1, const posit
 {
 	return ((p1->valid == p2->valid) &&
 		(p1->type == p2->type) &&
-		(fabsf(p1->x - p2->x) < FLT_EPSILON) &&
-		(fabsf(p1->y - p2->y) < FLT_EPSILON) &&
-		(fabsf(p1->z - p2->z) < FLT_EPSILON) &&
-		(p1->position_valid == p2->position_valid) &&
 		(fabsf(p1->vx - p2->vx) < FLT_EPSILON) &&
 		(fabsf(p1->vy - p2->vy) < FLT_EPSILON) &&
 		(fabsf(p1->vz - p2->vz) < FLT_EPSILON) &&

src/modules/rover_pos_control/RoverPositionControl.cpp

@@ -319,18 +319,15 @@ RoverPositionControl::control_position(const matrix::Vector2d &current_position,
 }
 
 void
-RoverPositionControl::control_velocity(const matrix::Vector3f &current_velocity,
+RoverPositionControl::control_velocity(const matrix::Vector3f &current_velocity)
-				       const position_setpoint_triplet_s &pos_sp_triplet)
 {
-
+	const Vector3f desired_velocity{_trajectory_setpoint.vx, _trajectory_setpoint.vy, _trajectory_setpoint.vz};
 	float dt = 0.01; // Using non zero value to a avoid division by zero
 
 	const float mission_throttle = _param_throttle_cruise.get();
-	const matrix::Vector3f desired_velocity{pos_sp_triplet.current.vx, pos_sp_triplet.current.vy, pos_sp_triplet.current.vz};
 	const float desired_speed = desired_velocity.norm();
 
 	if (desired_speed > 0.01f) {
-
 		const Dcmf R_to_body(Quatf(_vehicle_att.q).inversed());
 		const Vector3f vel = R_to_body * Vector3f(current_velocity(0), current_velocity(1), current_velocity(2));
 
@@ -344,13 +341,12 @@ RoverPositionControl::control_velocity(const matrix::Vector3f &current_velocity,
 
 		Vector3f desired_body_velocity;
 
-		if (pos_sp_triplet.current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_BODY_NED) {
+		if (_velocity_frame == VelocityFrame::NED) {
 			desired_body_velocity = desired_velocity;
 
 		} else {
 			// If the frame of the velocity setpoint is unknown, assume it is in local frame
 			desired_body_velocity = R_to_body * desired_velocity;
-
 		}
 
 		const float desired_theta = atan2f(desired_body_velocity(1), desired_body_velocity(0));
@@ -363,7 +359,6 @@ RoverPositionControl::control_velocity(const matrix::Vector3f &current_velocity,
 
 		_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = 0.0f;
 		_act_controls.control[actuator_controls_s::INDEX_YAW] = 0.0f;
-
 	}
 }
 
@@ -421,12 +416,14 @@ RoverPositionControl::Run()
 					_global_local_alt0 = _local_pos.ref_alt;
 				}
 
+				_trajectory_setpoint_sub.update(&_trajectory_setpoint);
+
 				// local -> global
 				map_projection_reproject(&_global_local_proj_ref,
-							 _pos_sp_triplet.current.x, _pos_sp_triplet.current.y,
+							 _trajectory_setpoint.x, _trajectory_setpoint.y,
 							 &_pos_sp_triplet.current.lat, &_pos_sp_triplet.current.lon);
 
-				_pos_sp_triplet.current.alt = _global_local_alt0 - _pos_sp_triplet.current.z;
+				_pos_sp_triplet.current.alt = _global_local_alt0 - _trajectory_setpoint.z;
 				_pos_sp_triplet.current.valid = true;
 			}
 
@@ -445,7 +442,7 @@ RoverPositionControl::Run()
 					float turn_distance = _param_l1_distance.get(); //_gnd_control.switch_distance(100.0f);
 
 					// publish status
-					position_controller_status_s pos_ctrl_status = {};
+					position_controller_status_s pos_ctrl_status{};
 
 					pos_ctrl_status.nav_roll = 0.0f;
 					pos_ctrl_status.nav_pitch = 0.0f;
@@ -463,14 +460,11 @@ RoverPositionControl::Run()
 					pos_ctrl_status.timestamp = hrt_absolute_time();
 
 					_pos_ctrl_status_pub.publish(pos_ctrl_status);
-
-
 				}
 
 			} else if (!_control_mode.flag_control_manual_enabled && _control_mode.flag_control_velocity_enabled) {
-
+				_trajectory_setpoint_sub.update(&_trajectory_setpoint);
-				control_velocity(current_velocity, _pos_sp_triplet);
+				control_velocity(current_velocity);
-
 			}
 		}
 

src/modules/rover_pos_control/RoverPositionControl.hpp

@@ -70,6 +70,7 @@
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
 
 using matrix::Dcmf;
@@ -112,6 +113,7 @@ private:
 	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */
 	uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)};
 	uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)};
+	uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
 
 	manual_control_setpoint_s		_manual_control_setpoint{};			    /**< r/c channel data */
 	position_setpoint_triplet_s		_pos_sp_triplet{};		/**< triplet of mission items */
@@ -121,6 +123,7 @@ private:
 	vehicle_local_position_s		_local_pos{};			/**< global vehicle position */
 	actuator_controls_s				_act_controls{};		/**< direct control of actuators */
 	vehicle_attitude_s				_vehicle_att{};
+	vehicle_local_position_setpoint_s       _trajectory_setpoint{};
 
 	uORB::SubscriptionData<vehicle_acceleration_s>		_vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
 
@@ -155,6 +158,11 @@ private:
 	/* previous waypoint */
 	matrix::Vector2d _prev_wp{0, 0};
 
+	enum class VelocityFrame {
+		NED,
+		BODY,
+	} _velocity_frame{VelocityFrame::NED};
+
 	float _manual_yaw_sp{0.0};
 	bool _reset_yaw_sp{true};
 
@@ -199,7 +207,7 @@ private:
 	 */
 	bool		control_position(const matrix::Vector2d &global_pos, const matrix::Vector3f &ground_speed,
 					 const position_setpoint_triplet_s &_pos_sp_triplet);
-	void		control_velocity(const matrix::Vector3f &current_velocity, const position_setpoint_triplet_s &pos_sp_triplet);
+	void		control_velocity(const matrix::Vector3f &current_velocity);
 	void		control_attitude(const vehicle_attitude_s &att, const vehicle_attitude_setpoint_s &att_sp);
 
 };

src/modules/uuv_pos_control/uuv_pos_control.cpp

@@ -178,21 +178,16 @@ void UUVPOSControl::Run()
 		    && _vcontrol_mode.flag_control_attitude_enabled
 		    && _vcontrol_mode.flag_control_rates_enabled) {
 
-
 			_vehicle_attitude_sub.update(&_vehicle_attitude);//get current vehicle attitude
-			_pos_sp_triplet_sub.update(&_pos_setpoint);
+			_trajectory_setpoint_sub.update(&_trajectory_setpoint);
 
-			float roll_des, pitch_des, yaw_des, x_pos_des, y_pos_des, z_pos_des;
+			float roll_des = 0;
-
+			float pitch_des = 0;
-
+			float yaw_des = _trajectory_setpoint.yaw;
-			roll_des = 0;
-			pitch_des = 0;
-			yaw_des = _pos_setpoint.current.yaw;
-
-			x_pos_des = _pos_setpoint.current.x;
-			y_pos_des = _pos_setpoint.current.y;
-			z_pos_des = _pos_setpoint.current.z;
 
+			float x_pos_des = _trajectory_setpoint.x;
+			float y_pos_des = _trajectory_setpoint.y;
+			float z_pos_des = _trajectory_setpoint.z;
 
 			//stabilization controller(keep pos and hold depth + angle) vs position controller(global + yaw)
 			if (_param_stabilization.get() == 0) {
@@ -203,9 +198,6 @@ void UUVPOSControl::Run()
 				stabilization_controller_6dof(x_pos_des, y_pos_des, z_pos_des,
 							      roll_des, pitch_des, yaw_des, _vehicle_attitude, vlocal_pos);
 			}
-
-
-
 		}
 	}
 

src/modules/uuv_pos_control/uuv_pos_control.hpp

@@ -62,9 +62,9 @@
 #include <uORB/Publication.hpp>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/parameter_update.h>
-#include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
@@ -105,16 +105,15 @@ private:
 
 	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};	/**< current vehicle attitude */
 	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};	/**< notification of manual control updates */
+	uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
 	uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)};		/**< vehicle status subscription */
-	uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)}; /**< position setpoint */
 
 	uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)};
 
-
 	//actuator_controls_s _actuators {}; /**< actuator control inputs */
 	manual_control_setpoint_s _manual_control_setpoint {}; /**< r/c channel data */
 	vehicle_attitude_s _vehicle_attitude {}; /**< vehicle attitude */
-	position_setpoint_triplet_s _pos_setpoint {}; /**< vehicle position setpoint */
+	vehicle_local_position_setpoint_s _trajectory_setpoint{}; /**< vehicle position setpoint */
 	vehicle_control_mode_s _vcontrol_mode {}; /**< vehicle control mode */
 
 	perf_counter_t	_loop_perf; /**< loop performance counter */