diff --git a/msg/trajectory_waypoint.msg b/msg/trajectory_waypoint.msg index 22d222423a30..6ea9bae4d34e 100644 --- a/msg/trajectory_waypoint.msg +++ b/msg/trajectory_waypoint.msg @@ -10,3 +10,4 @@ float32 yaw float32 yaw_speed bool point_valid +uint8 type diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp index 8f9f3ac987e9..f95535e543a4 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp +++ b/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; diff --git a/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp b/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp index 1387609ef2f2..173291919e33 100644 --- a/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp +++ b/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); } diff --git a/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp b/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp index 67c4c1c6f2a2..ed35fe45c9cd 100644 --- a/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp +++ b/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); } diff --git a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp index fbdcb8f98231..b499d1f66e79 100644 --- a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp +++ b/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; diff --git a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp index f5e2803429c1..df0a9279f4b9 100644 --- a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp +++ b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp @@ -85,15 +85,18 @@ 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. @@ -101,10 +104,9 @@ class ObstacleAvoidance : public ModuleParams * @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 */ diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 56345cb8e09a..862b91cb2cba 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/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++; } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 818d0c0a4528..f68d6d393f64 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/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) { diff --git a/src/modules/mc_pos_control/CMakeLists.txt b/src/modules/mc_pos_control/CMakeLists.txt index b7654d6a748b..77353b8f9c82 100644 --- a/src/modules/mc_pos_control/CMakeLists.txt +++ b/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