fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

add point type (mavlink command associated with wp) in Obstacle Avoidance interface

Browse Source
This commit is contained in:
Martina Rivizzigno 2019-06-24 22:15:34 +02:00 committed by Beat Küng
parent 4d5f922e7a
commit 39e59d6cc4
8 changed files with 43 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/trajectory_waypoint.msg
View File

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

5
src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
View File

@ -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;

2
src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp
View File

@ -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);
}

2
src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp
View File

@ -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);
}

23
src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
View File

@ -46,11 +46,11 @@ static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US = 500_ms;
static constexpr uint64_t TIME_BEFORE_FAILSAFE = 500_ms;
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, UINT8_MAX, false, {0}},
{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, UINT8_MAX, false, {0}},
{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, UINT8_MAX, false, {0}},
{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, UINT8_MAX, false, {0}},
{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, UINT8_MAX, false, {0}}
}
};
@ -126,13 +126,14 @@ 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;
_curr_wp = curr_wp;
_ext_yaw_active = ext_yaw_active;
_curr_wp_type = wp_type;
curr_wp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].position);
Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].velocity);
@ -141,6 +142,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 = _curr_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 +153,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 +166,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 = {};
@ -187,7 +190,7 @@ void ObstacleAvoidance::checkAvoidanceProgress(const Vector3f &pos, const Vector
const float pos_to_target_z = fabsf(_curr_wp(2) - _position(2));
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) {
&& _curr_wp_type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
// vehicle above or below the target waypoint
pos_control_status.altitude_acceptance = pos_to_target_z + 0.5f;
}

11
src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp
View File

@ -85,15 +85,18 @@ public:
* @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 @@ public:
* @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:
@ -128,6 +130,7 @@ private:
systemlib::Hysteresis _avoidance_point_not_valid_hysteresis{false}; /**< becomes true if the companion doesn't start sending valid setpoints */
bool _ext_yaw_active = false; /**< true, if external yaw handling is active */
int _curr_wp_type = 0;
/**
* Publishes vehicle command.

17
src/modules/mavlink/mavlink_messages.cpp
View File

@ -3691,6 +3691,23 @@ protected:
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++;
}

1
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -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) {