Log vehicle local position setpoints

This commit enables the local position setpoints to be logged by publishing vehicle_local_position_setpoint

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -863,6 +863,8 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
 	if (use_tecs_pitch) {
 		_att_sp.pitch_body = get_tecs_pitch();
 	}
+
+	publishLocalPositionSetpoint(current_sp);
 }
 
 void
@@ -1987,17 +1989,16 @@ FixedwingPositionControl::Run()
 		_alt_reset_counter = _local_pos.vz_reset_counter;
 		_pos_reset_counter = _local_pos.vxy_reset_counter;
 
+		// Convert Local setpoints to global setpoints
+		if (!_global_local_proj_ref.isInitialized()
+		    || (_global_local_proj_ref.getProjectionReferenceTimestamp() != _local_pos.ref_timestamp)) {
 
-		if (_control_mode.flag_control_offboard_enabled) {
-			// Convert Local setpoints to global setpoints
-			if (!_global_local_proj_ref.isInitialized()
-			    || (_global_local_proj_ref.getProjectionReferenceTimestamp() != _local_pos.ref_timestamp)) {
-
-				_global_local_proj_ref.initReference(_local_pos.ref_lat, _local_pos.ref_lon,
-								     _local_pos.ref_timestamp);
-				_global_local_alt0 = _local_pos.ref_alt;
-			}
+			_global_local_proj_ref.initReference(_local_pos.ref_lat, _local_pos.ref_lon,
+							     _local_pos.ref_timestamp);
+			_global_local_alt0 = _local_pos.ref_alt;
+		}
 
+		if (_control_mode.flag_control_offboard_enabled) {
 			vehicle_local_position_setpoint_s trajectory_setpoint;
 
 			if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) {
@@ -2321,6 +2322,22 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
 	tecs_status_publish();
 }
 
+void FixedwingPositionControl::publishLocalPositionSetpoint(const position_setpoint_s current_waypoint)
+{
+	if (_global_local_proj_ref.isInitialized()) {
+		Vector2f current_setpoint = _global_local_proj_ref.project(current_waypoint.lat, current_waypoint.lon);
+		vehicle_local_position_setpoint_s local_position_setpoint{};
+		local_position_setpoint.x = current_setpoint(0);
+		local_position_setpoint.y = current_setpoint(1);
+		local_position_setpoint.z = _global_local_alt0 - current_waypoint.alt;
+		local_position_setpoint.thrust[0] = _att_sp.thrust_body[0];
+		local_position_setpoint.thrust[1] = _att_sp.thrust_body[1];
+		local_position_setpoint.thrust[2] = _att_sp.thrust_body[2];
+		local_position_setpoint.timestamp = hrt_absolute_time();
+		_local_pos_sp_pub.publish(local_position_setpoint);
+	}
+}
+
 void FixedwingPositionControl::publishOrbitStatus(const position_setpoint_s pos_sp)
 {
 	orbit_status_s orbit_status{};

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -154,6 +154,7 @@ private:
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 
 	uORB::Publication<vehicle_attitude_setpoint_s>		_attitude_sp_pub;
+	uORB::Publication<vehicle_local_position_setpoint_s> 	_local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)};	///< vehicle local position setpoint publication
 	uORB::Publication<position_controller_status_s>		_pos_ctrl_status_pub{ORB_ID(position_controller_status)};			///< navigation capabilities publication
 	uORB::Publication<position_controller_landing_status_s>	_pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)};	///< landing status publication
 	uORB::Publication<tecs_status_s>			_tecs_status_pub{ORB_ID(tecs_status)};						///< TECS status publication
@@ -285,6 +286,7 @@ private:
 	void		status_publish();
 	void		landing_status_publish();
 	void		tecs_status_publish();
+	void 		publishLocalPositionSetpoint(const position_setpoint_s current_waypoint);
 
 	void		abort_landing(bool abort);