fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 19:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

remove empty_trajectory_waypoint and getter method for avoidance

Browse Source 
waypoints in FlightTasks. In obstacle avoidace library reset desired
avoidance waypoints after publication
This commit is contained in:
Martina Rivizzigno
2019-04-04 16:41:26 +02:00
committed by bresch
parent 34b0f33098
commit e464502d2d
3 changed files with 12 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
+12 -2
View File
@@ -46,10 +46,19 @@ using namespace time_literals;
/** Timeout in us for trajectory data to get considered invalid */
static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US = 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}}
}
};
ObstacleAvoidance::ObstacleAvoidance(ModuleParams *parent) :
ModuleParams(parent)
{
_desired_waypoint = empty_trajectory_waypoint;
}
ObstacleAvoidance::~ObstacleAvoidance()
@@ -123,10 +132,11 @@ void ObstacleAvoidance::updateAvoidanceSetpoints(const Vector3f &pos_sp, const V
{
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].point_valid = true;
_publish_avoidance_desired_waypoint();
// TODO: reset all fields to NaN
_desired_waypoint = empty_trajectory_waypoint;
}
void ObstacleAvoidance::checkAvoidanceProgress(const Vector3f &pos, const Vector3f &prev_wp,