Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 11:59:17 +08:00
Code Issues Actions 21 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-03-03 15:04:40 +01: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/FlightTasks.cpp
-15
View File
@@ -57,21 +57,6 @@ const landing_gear_s FlightTasks::getGear()
} }
} }
const vehicle_trajectory_waypoint_s FlightTasks::getAvoidanceWaypoint()
{
if (isAnyTaskActive()) {
return _current_task.task->getAvoidanceWaypoint();
} else {
return FlightTask::empty_trajectory_waypoint;
}
}
const vehicle_trajectory_waypoint_s &FlightTasks::getEmptyAvoidanceWaypoint()
{
return FlightTask::empty_trajectory_waypoint;
}
int FlightTasks::switchTask(FlightTaskIndex new_task_index) int FlightTasks::switchTask(FlightTaskIndex new_task_index)
{ {
// switch to the running task, nothing to do // switch to the running task, nothing to do
src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp
-9
View File
@@ -8,14 +8,6 @@ const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NA
const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {}}; const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {}};
const landing_gear_s FlightTask::empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_KEEP, {}}; const landing_gear_s FlightTask::empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_KEEP, {}};
const vehicle_trajectory_waypoint_s FlightTask::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}}
}
};
bool FlightTask::initializeSubscriptions(SubscriptionArray &subscription_array) bool FlightTask::initializeSubscriptions(SubscriptionArray &subscription_array)
{ {
@@ -92,7 +84,6 @@ void FlightTask::_resetSetpoints()
_jerk_setpoint.setAll(NAN); _jerk_setpoint.setAll(NAN);
_thrust_setpoint.setAll(NAN); _thrust_setpoint.setAll(NAN);
_yaw_setpoint = _yawspeed_setpoint = NAN; _yaw_setpoint = _yawspeed_setpoint = NAN;
_desired_waypoint = FlightTask::empty_trajectory_waypoint;
} }
void FlightTask::_evaluateVehicleLocalPosition() void FlightTask::_evaluateVehicleLocalPosition()
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 */ /** Timeout in us for trajectory data to get considered invalid */
static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US = 500_ms; 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) : ObstacleAvoidance::ObstacleAvoidance(ModuleParams *parent) :
ModuleParams(parent) ModuleParams(parent)
{ {
_desired_waypoint = empty_trajectory_waypoint;
} }
ObstacleAvoidance::~ObstacleAvoidance() 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); 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); 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(); _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, void ObstacleAvoidance::checkAvoidanceProgress(const Vector3f &pos, const Vector3f &prev_wp,