navigator: publish navigator mission item changes for logging

- new msg navigator_mission_item for inspecting navigator mission item processing

msg/CMakeLists.txt

@@ -85,6 +85,7 @@ set(msg_files
 	mission_result.msg
 	mount_orientation.msg
 	multirotor_motor_limits.msg
+	navigator_mission_item.msg
 	obstacle_distance.msg
 	offboard_control_mode.msg
 	onboard_computer_status.msg

msg/navigator_mission_item.msg

@@ -0,0 +1,25 @@
+uint64 timestamp                 # time since system start (microseconds)
+
+uint32 instance_count            # Instance count of this mission. Increments monotonically whenever the mission is modified
+
+uint16 sequence_current          # Sequence of the current mission item
+
+uint16 nav_cmd
+
+float32 latitude
+float32 longitude
+
+float32 time_inside              # time that the MAV should stay inside the radius before advancing in seconds
+float32 acceptance_radius        # default radius in which the mission is accepted as reached in meters
+float32 loiter_radius            # loiter radius in meters, 0 for a VTOL to hover, negative for counter-clockwise
+float32 yaw                      # in radians NED -PI..+PI, NAN means don't change yaw
+float32 altitude                 # altitude in meters (AMSL)
+
+uint8 frame                      # mission frame
+uint8 origin                     # mission item origin (onboard or mavlink)
+
+bool loiter_exit_xtrack          # exit xtrack location: 0 for center of loiter wp, 1 for exit location
+bool force_heading               # heading needs to be reached
+bool altitude_is_relative        # true if altitude is relative from start point
+bool autocontinue                # true if next waypoint should follow after this one
+bool vtol_back_transition        # part of the vtol back transition sequence

msg/tools/uorb_rtps_message_ids.yaml

@@ -287,6 +287,8 @@ rtps:
     id: 136
   - msg: sensor_gyro_fft
     id: 137
+  - msg: navigator_mission_item
+    id: 138
   ########## multi topics: begin ##########
   - msg: actuator_controls_0
     id: 150

src/modules/logger/logged_topics.cpp

@@ -70,6 +70,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("manual_control_setpoint", 200);
 	add_topic("mission");
 	add_topic("mission_result");
+	add_topic("navigator_mission_item");
 	add_topic("offboard_control_mode", 100);
 	add_topic("onboard_computer_status", 10);
 	add_topic("position_controller_status", 500);

src/modules/navigator/mission.cpp

@@ -323,6 +323,7 @@ Mission::set_execution_mode(const uint8_t mode)
 					position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 					pos_sp_triplet->previous = pos_sp_triplet->current;
 					generate_waypoint_from_heading(&pos_sp_triplet->current, _mission_item.yaw);
+					publish_navigator_mission_item(); // for logging
 					_navigator->set_position_setpoint_triplet_updated();
 					issue_command(_mission_item);
 				}
@@ -661,7 +662,9 @@ Mission::set_mission_items()
 			user_feedback_done = true;
 		}
 
+		publish_navigator_mission_item(); // for logging
 		_navigator->set_position_setpoint_triplet_updated();
+
 		return;
 	}
 
@@ -1145,6 +1148,7 @@ Mission::set_mission_items()
 		}
 	}
 
+	publish_navigator_mission_item(); // for logging
 	_navigator->set_position_setpoint_triplet_updated();
 }
 
@@ -1335,6 +1339,7 @@ Mission::heading_sp_update()
 		}
 
 		// we set yaw directly so we can run this in parallel to the FOH update
+		publish_navigator_mission_item();
 		_navigator->set_position_setpoint_triplet_updated();
 	}
 }
@@ -1354,6 +1359,7 @@ Mission::cruising_speed_sp_update()
 
 	pos_sp_triplet->current.cruising_speed = cruising_speed;
 
+	publish_navigator_mission_item();
 	_navigator->set_position_setpoint_triplet_updated();
 }
 
@@ -1384,6 +1390,7 @@ Mission::do_abort_landing()
 
 	mission_apply_limitation(_mission_item);
 	mission_item_to_position_setpoint(_mission_item, &_navigator->get_position_setpoint_triplet()->current);
+	publish_navigator_mission_item(); // for logging
 	_navigator->set_position_setpoint_triplet_updated();
 
 	mavlink_log_info(_navigator->get_mavlink_log_pub(), "Holding at %d m above landing.",
@@ -1826,3 +1833,33 @@ bool Mission::position_setpoint_equal(const position_setpoint_s *p1, const posit
 				&& !PX4_ISFINITE(p2->cruising_throttle))));
 
 }
+
+void Mission::publish_navigator_mission_item()
+{
+	navigator_mission_item_s navigator_mission_item{};
+
+	navigator_mission_item.instance_count = _navigator->mission_instance_count();
+	navigator_mission_item.sequence_current = _current_mission_index;
+	navigator_mission_item.nav_cmd = _mission_item.nav_cmd;
+	navigator_mission_item.latitude = _mission_item.lat;
+	navigator_mission_item.longitude = _mission_item.lon;
+	navigator_mission_item.altitude = _mission_item.altitude;
+
+	navigator_mission_item.time_inside = get_time_inside(_mission_item);
+	navigator_mission_item.acceptance_radius = _mission_item.acceptance_radius;
+	navigator_mission_item.loiter_radius = _mission_item.loiter_radius;
+	navigator_mission_item.yaw = _mission_item.yaw;
+
+	navigator_mission_item.frame = _mission_item.frame;
+	navigator_mission_item.frame = _mission_item.origin;
+
+	navigator_mission_item.loiter_exit_xtrack = _mission_item.loiter_exit_xtrack;
+	navigator_mission_item.force_heading = _mission_item.force_heading;
+	navigator_mission_item.altitude_is_relative = _mission_item.altitude_is_relative;
+	navigator_mission_item.autocontinue = _mission_item.autocontinue;
+	navigator_mission_item.vtol_back_transition = _mission_item.vtol_back_transition;
+
+	navigator_mission_item.timestamp = hrt_absolute_time();
+
+	_navigator_mission_item_pub.publish(navigator_mission_item);
+}

src/modules/navigator/mission.h

@@ -57,6 +57,7 @@
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
+#include <uORB/topics/navigator_mission_item.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_status.h>
@@ -230,12 +231,16 @@ private:
 
 	bool position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const;
 
+	void publish_navigator_mission_item();
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::MIS_DIST_1WP>) _param_mis_dist_1wp,
 		(ParamFloat<px4::params::MIS_DIST_WPS>) _param_mis_dist_wps,
 		(ParamInt<px4::params::MIS_MNT_YAW_CTL>) _param_mis_mnt_yaw_ctl
 	)
 
+	uORB::Publication<navigator_mission_item_s> _navigator_mission_item_pub{ORB_ID::navigator_mission_item};
+
 	uORB::Subscription	_mission_sub{ORB_ID(mission)};		/**< mission subscription */
 	mission_s		_mission {};
 

src/modules/navigator/navigator.h

@@ -265,6 +265,7 @@ public:
 	orb_advert_t	*get_mavlink_log_pub() { return &_mavlink_log_pub; }
 
 	void		increment_mission_instance_count() { _mission_result.instance_count++; }
+	int		mission_instance_count() const { return _mission_result.instance_count; }
 
 	void 		set_mission_failure(const char *reason);