OA: interface update

msg/trajectory_waypoint.msg

@@ -10,3 +10,4 @@ float32 yaw
 float32 yaw_speed
 
 bool point_valid
+uint8 type

src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp

@@ -292,9 +292,8 @@ bool FlightTaskAuto::_evaluateTriplets()
 				_triplet_next_wp,
 				_sub_triplet_setpoint->get().next.yaw,
 				_sub_triplet_setpoint->get().next.yawspeed_valid ? _sub_triplet_setpoint->get().next.yawspeed : NAN,
-				_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active());
-		_obstacle_avoidance.checkAvoidanceProgress(_position, _triplet_prev_wp, _target_acceptance_radius, _closest_pt,
-				_sub_triplet_setpoint->get().current.type);
+				_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);
 	}
 
 	return true;

src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp

@@ -87,7 +87,7 @@ bool FlightTaskAutoMapper::update()
 	}
 
 	if (_param_com_obs_avoid.get()) {
-		_obstacle_avoidance.updateAvoidanceDesiredSetpoints(_position_setpoint, _velocity_setpoint);
+		_obstacle_avoidance.updateAvoidanceDesiredSetpoints(_position_setpoint, _velocity_setpoint, (int)_type);
 		_obstacle_avoidance.injectAvoidanceSetpoints(_position_setpoint, _velocity_setpoint, _yaw_setpoint,
 				_yawspeed_setpoint);
 	}

src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp

@@ -92,7 +92,7 @@ bool FlightTaskAutoMapper2::update()
 	}
 
 	if (_param_com_obs_avoid.get()) {
-		_obstacle_avoidance.updateAvoidanceDesiredSetpoints(_position_setpoint, _velocity_setpoint);
+		_obstacle_avoidance.updateAvoidanceDesiredSetpoints(_position_setpoint, _velocity_setpoint, (int)_type);
 		_obstacle_avoidance.injectAvoidanceSetpoints(_position_setpoint, _velocity_setpoint, _yaw_setpoint,
 				_yawspeed_setpoint);
 	}

src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp

@@ -44,13 +44,14 @@ using namespace time_literals;
 static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US = 500_ms;
 /** If Flighttask fails, keep 0.5 seconds the current setpoint before going into failsafe land */
 static constexpr uint64_t TIME_BEFORE_FAILSAFE = 500_ms;
+static constexpr uint64_t Z_PROGRESS_TIMEOUT_US = 2_s;
 
 const vehicle_trajectory_waypoint_s empty_trajectory_waypoint = {0, 0, {0, 0, 0, 0, 0, 0, 0},
-	{	{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, {0, 0, 0}},
-		{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, {0, 0, 0}},
-		{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, {0, 0, 0}},
-		{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, {0, 0, 0}},
-		{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, {0, 0, 0}}
+	{	{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}},
+		{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}},
+		{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}},
+		{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}},
+		{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}}
 	}
 };
 
@@ -60,6 +61,7 @@ ObstacleAvoidance::ObstacleAvoidance(ModuleParams *parent) :
 	_desired_waypoint = empty_trajectory_waypoint;
 	_failsafe_position.setNaN();
 	_avoidance_point_not_valid_hysteresis.set_hysteresis_time_from(false, TIME_BEFORE_FAILSAFE);
+	_no_progress_z_hysteresis.set_hysteresis_time_from(false, Z_PROGRESS_TIMEOUT_US);
 
 }
 
@@ -126,8 +128,8 @@ void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel
 }
 
 void ObstacleAvoidance::updateAvoidanceDesiredWaypoints(const Vector3f &curr_wp, const float curr_yaw,
-		const float curr_yawspeed,
-		const Vector3f &next_wp, const float next_yaw, const float next_yawspeed, const bool ext_yaw_active)
+		const float curr_yawspeed, const Vector3f &next_wp, const float next_yaw, const float next_yawspeed,
+		const bool ext_yaw_active, const int wp_type)
 {
 	_desired_waypoint.timestamp = hrt_absolute_time();
 	_desired_waypoint.type = vehicle_trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS;
@@ -141,6 +143,7 @@ void ObstacleAvoidance::updateAvoidanceDesiredWaypoints(const Vector3f &curr_wp,
 	_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw = curr_yaw;
 	_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw_speed = curr_yawspeed;
 	_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].point_valid = true;
+	_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].type = wp_type;
 
 	next_wp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].position);
 	Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].velocity);
@@ -151,10 +154,11 @@ void ObstacleAvoidance::updateAvoidanceDesiredWaypoints(const Vector3f &curr_wp,
 	_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].point_valid = true;
 }
 
-void ObstacleAvoidance::updateAvoidanceDesiredSetpoints(const Vector3f &pos_sp, const Vector3f &vel_sp)
+void ObstacleAvoidance::updateAvoidanceDesiredSetpoints(const Vector3f &pos_sp, const Vector3f &vel_sp, const int type)
 {
 	pos_sp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position);
 	vel_sp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity);
+	_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].type = type;
 	_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid = true;
 
 	_pub_traj_wp_avoidance_desired.publish(_desired_waypoint);
@@ -163,7 +167,7 @@ void ObstacleAvoidance::updateAvoidanceDesiredSetpoints(const Vector3f &pos_sp,
 }
 
 void ObstacleAvoidance::checkAvoidanceProgress(const Vector3f &pos, const Vector3f &prev_wp,
-		float target_acceptance_radius, const Vector2f &closest_pt, const int wp_type)
+		float target_acceptance_radius, const Vector2f &closest_pt)
 {
 	_position = pos;
 	position_controller_status_s pos_control_status = {};
@@ -186,12 +190,17 @@ void ObstacleAvoidance::checkAvoidanceProgress(const Vector3f &pos, const Vector
 
 	const float pos_to_target_z = fabsf(_curr_wp(2) - _position(2));
 
+	bool no_progress_z = (pos_to_target_z > _prev_pos_to_target_z);
+	_no_progress_z_hysteresis.set_state_and_update(no_progress_z, hrt_absolute_time());
+
 	if (pos_to_target.length() < target_acceptance_radius && pos_to_target_z > _param_nav_mc_alt_rad.get()
-	    && wp_type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
+	    && _no_progress_z_hysteresis.get_state()) {
 		// vehicle above or below the target waypoint
 		pos_control_status.altitude_acceptance = pos_to_target_z + 0.5f;
 	}
 
+	_prev_pos_to_target_z = pos_to_target_z;
+
 	// do not check for waypoints yaw acceptance in navigator
 	pos_control_status.yaw_acceptance = NAN;
 

src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp

@@ -85,26 +85,28 @@ class ObstacleAvoidance : public ModuleParams
 	 * @param next_wp, next position triplet
 	 * @param next_yaw, next yaw triplet
 	 * @param next_yawspeed, next yaw speed triplet
+	 * @param wp_type, current triplet type
 	 */
 	void updateAvoidanceDesiredWaypoints(const matrix::Vector3f &curr_wp, const float curr_yaw, const float curr_yawspeed,
-					     const matrix::Vector3f &next_wp, const float next_yaw, const float next_yawspeed, const bool ext_yaw_active);
+					     const matrix::Vector3f &next_wp, const float next_yaw, const float next_yawspeed, const bool ext_yaw_active,
+					     const int wp_type);
 	/**
 	 * Updates the desired setpoints to send to the obstacle avoidance system.
 	 * @param pos_sp, desired position setpoint computed by the active FlightTask
 	 * @param vel_sp, desired velocity setpoint computed by the active FlightTask
+	 * @param type, current triplet type
 	 */
-	void updateAvoidanceDesiredSetpoints(const matrix::Vector3f &pos_sp, const matrix::Vector3f &vel_sp);
+	void updateAvoidanceDesiredSetpoints(const matrix::Vector3f &pos_sp, const matrix::Vector3f &vel_sp, const int type);
 
 	/**
 	 * Checks the vehicle progress between previous and current position waypoint of the triplet.
 	 * @param pos, vehicle position
 	 * @param prev_wp, previous position triplet
 	 * @param target_acceptance_radius, current position triplet xy acceptance radius
 	 * @param closest_pt, closest point to the vehicle on the line previous-current position triplet
-	 * @param wp_type, current triplet type
 	 */
 	void checkAvoidanceProgress(const matrix::Vector3f &pos, const matrix::Vector3f &prev_wp,
-				    float target_acceptance_radius, const matrix::Vector2f &closest_pt, const int wp_type);
+				    float target_acceptance_radius, const matrix::Vector2f &closest_pt);
 
 private:
 
@@ -126,6 +128,9 @@ class ObstacleAvoidance : public ModuleParams
 	matrix::Vector3f _failsafe_position = {}; /**< vehicle position when entered in failsafe */
 
 	systemlib::Hysteresis _avoidance_point_not_valid_hysteresis{false}; /**< becomes true if the companion doesn't start sending valid setpoints */
+	systemlib::Hysteresis _no_progress_z_hysteresis{false}; /**< becomes true if the vehicle is not making progress towards the z component of the goal */
+
+	float _prev_pos_to_target_z = -1.f; /**< z distance to the goal */
 
 	bool _ext_yaw_active = false; /**< true, if external yaw handling is active */
 

src/modules/mavlink/mavlink_messages.cpp

@@ -3691,6 +3691,23 @@ class MavlinkStreamTrajectoryRepresentationWaypoints: public MavlinkStream
 				msg.pos_yaw[i] = traj_wp_avoidance_desired.waypoints[i].yaw;
 				msg.vel_yaw[i] = traj_wp_avoidance_desired.waypoints[i].yaw_speed;
 
+				switch (traj_wp_avoidance_desired.waypoints[i].type) {
+				case position_setpoint_s::SETPOINT_TYPE_TAKEOFF:
+					msg.command[i] = vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF;
+					break;
+
+				case position_setpoint_s::SETPOINT_TYPE_LOITER:
+					msg.command[i] = vehicle_command_s::VEHICLE_CMD_NAV_LOITER_UNLIM;
+					break;
+
+				case position_setpoint_s::SETPOINT_TYPE_LAND:
+					msg.command[i] = vehicle_command_s::VEHICLE_CMD_NAV_LAND;
+					break;
+
+				default:
+					msg.command[i] = UINT16_MAX;
+				}
+
 				if (traj_wp_avoidance_desired.waypoints[i].point_valid) {
 					number_valid_points++;
 				}

src/modules/mavlink/mavlink_receiver.cpp

@@ -1713,6 +1713,7 @@ MavlinkReceiver::handle_message_trajectory_representation_waypoints(mavlink_mess
 		trajectory_waypoint.waypoints[i].yaw = trajectory.pos_yaw[i];
 		trajectory_waypoint.waypoints[i].yaw_speed = trajectory.vel_yaw[i];
 
+		trajectory_waypoint.waypoints[i].type = UINT8_MAX;
 	}
 
 	for (int i = 0; i < number_valid_points; ++i) {

src/modules/mc_pos_control/CMakeLists.txt

@@ -37,7 +37,7 @@ px4_add_module(
 	MODULE modules__mc_pos_control
 	MAIN mc_pos_control
 	COMPILE_FLAGS
-	STACK_MAIN 1300
+	STACK_MAIN 1500
 	SRCS
 		mc_pos_control_main.cpp
 		PositionControl.cpp