vehicle trajectory limit unnecessary updates and reduce logging (#10347)

- fixes #10345

src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp

@@ -194,13 +194,13 @@ protected:
 	 * Vehicle constraints.
 	 * The constraints can vary with tasks.
 	 */
-	vehicle_constraints_s _constraints;
+	vehicle_constraints_s _constraints{};
 
 	/**
 	 * Desired waypoints.
 	 * Goals set by the FCU to be sent to the obstacle avoidance system.
 	 */
-	vehicle_trajectory_waypoint_s _desired_waypoint;
+	vehicle_trajectory_waypoint_s _desired_waypoint{};
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams,
 					(ParamFloat<px4::params::MPC_XY_VEL_MAX>) MPC_XY_VEL_MAX,

src/modules/logger/logger.cpp

@@ -643,8 +643,8 @@ void Logger::add_default_topics()
 	add_topic("vehicle_rates_setpoint", 30);
 	add_topic("vehicle_status", 200);
 	add_topic("vehicle_status_flags");
-	add_topic("vehicle_trajectory_waypoint");
+	add_topic("vehicle_trajectory_waypoint", 200);
-	add_topic("vehicle_trajectory_waypoint_desired");
+	add_topic("vehicle_trajectory_waypoint_desired", 200);
 	add_topic("vehicle_vision_attitude");
 	add_topic("vehicle_vision_position");
 	add_topic("vtol_vehicle_status", 200);

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -129,9 +129,8 @@ private:
 	vehicle_local_position_s			_local_pos{};		/**< vehicle local position */
 	vehicle_local_position_setpoint_s	_local_pos_sp{};		/**< vehicle local position setpoint */
 	home_position_s				_home_pos{}; 				/**< home position */
-	vehicle_trajectory_waypoint_s		_traj_wp_avoidance; /**< trajectory waypoint */
+	vehicle_trajectory_waypoint_s		_traj_wp_avoidance{}; /**< trajectory waypoint */
-	vehicle_trajectory_waypoint_s
+	vehicle_trajectory_waypoint_s		_traj_wp_avoidance_desired{}; /**< desired waypoints, inputs to an obstacle avoidance module */
-	_traj_wp_avoidance_desired; /**< desired waypoints, inputs to an obstacle avoidance module */
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::MPC_TKO_RAMP_T>) _takeoff_ramp_time, /**< time constant for smooth takeoff ramp */
@@ -275,7 +274,6 @@ private:
 	 */
 	void publish_avoidance_desired_waypoint();
 
-
 	/**
 	 * Shim for calling task_main from task_create.
 	 */
@@ -739,8 +737,6 @@ MulticopterPositionControl::task_main()
 			// they might conflict with each other such as in offboard attitude control.
 			publish_attitude();
 
-			publish_avoidance_desired_waypoint();
-
 		} else {
 			// no flighttask is active: set attitude setpoint to idle
 			_att_sp.roll_body = _att_sp.pitch_body = 0.0f;
@@ -1009,17 +1005,24 @@ MulticopterPositionControl::failsafe(vehicle_local_position_setpoint_s &setpoint
 void
 MulticopterPositionControl::update_avoidance_waypoint_desired(PositionControlStates &states)
 {
-	_traj_wp_avoidance_desired = _flight_tasks.getAvoidanceWaypoint();
+	if (MPC_OBS_AVOID.get()) {
+		const vehicle_trajectory_waypoint_s traj_wp_desired_new = _flight_tasks.getAvoidanceWaypoint();
 
-	_traj_wp_avoidance_desired.timestamp = hrt_absolute_time();
+		if (traj_wp_desired_new.timestamp > _traj_wp_avoidance_desired.timestamp) {
-	_traj_wp_avoidance_desired.type = vehicle_trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS;
+			_traj_wp_avoidance_desired = traj_wp_desired_new;
+			_traj_wp_avoidance_desired.timestamp = hrt_absolute_time();
+			_traj_wp_avoidance_desired.type = vehicle_trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS;
 
-	states.position.copyTo(_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position);
+			states.position.copyTo(_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position);
-	states.position.copyTo(_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity);
+			states.position.copyTo(_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity);
-	states.position.copyTo(_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].acceleration);
+			states.position.copyTo(_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].acceleration);
-	_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw = states.yaw;
+			_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw = states.yaw;
-	_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw_speed = NAN;
+			_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw_speed = NAN;
-	_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid = true;
+			_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid = true;
+
+			publish_avoidance_desired_waypoint();
+		}
+	}
 }
 
 void
@@ -1057,7 +1060,7 @@ MulticopterPositionControl::use_obstacle_avoidance()
 void
 MulticopterPositionControl::publish_avoidance_desired_waypoint()
 {
-	/* publish desired waypoint*/
+	// publish desired waypoint
 	if (_traj_wp_avoidance_desired_pub != nullptr) {
 		orb_publish(ORB_ID(vehicle_trajectory_waypoint_desired), _traj_wp_avoidance_desired_pub, &_traj_wp_avoidance_desired);
 
@@ -1065,16 +1068,11 @@ MulticopterPositionControl::publish_avoidance_desired_waypoint()
 		_traj_wp_avoidance_desired_pub = orb_advertise(ORB_ID(vehicle_trajectory_waypoint_desired),
 						 &_traj_wp_avoidance_desired);
 	}
-
-	//reset avoidance waypoint desired
-	_traj_wp_avoidance_desired = _flight_tasks.getEmptyAvoidanceWaypoint();
 }
 
-
 void
 MulticopterPositionControl::publish_attitude()
 {
-
 	_att_sp.timestamp = hrt_absolute_time();
 
 	if (_att_sp_pub != nullptr) {
@@ -1088,7 +1086,6 @@ MulticopterPositionControl::publish_attitude()
 void
 MulticopterPositionControl::publish_local_pos_sp()
 {
-
 	_local_pos_sp.timestamp = hrt_absolute_time();
 
 	// publish local position setpoint