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

FlightTaskAuto: add update method to get triplets for the trajectory

Browse Source 
interface
This commit is contained in:
Martina 2018-07-09 11:52:27 +02:00 committed by Daniel Agar
parent 7c7602873e
commit 17a08a9de7
2 changed files with 29 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -214,6 +214,7 @@ bool FlightTaskAuto::_evaluateTriplets()
if (triplet_update || (_current_state != previous_state)) {
_updateInternalWaypoints();
_updateAvoidanceWaypoints();
}
return true;
@ -264,6 +265,33 @@ void FlightTaskAuto::_set_heading_from_mode()
}
}
void FlightTaskAuto::_updateAvoidanceWaypoints()
{
_desired_waypoint.timestamp = hrt_absolute_time();
_target.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);
Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].acceleration);
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw = _sub_triplet_setpoint->get().current.yaw;
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw_speed =
_sub_triplet_setpoint->get().current.yawspeed_valid ?
_sub_triplet_setpoint->get().current.yawspeed : NAN;
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].point_valid = true;
_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);
Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].acceleration);
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw = _sub_triplet_setpoint->get().next.yaw;
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw_speed =
_sub_triplet_setpoint->get().next.yawspeed_valid ?
_sub_triplet_setpoint->get().next.yawspeed : NAN;
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].point_valid = true;
}
bool FlightTaskAuto::_isFinite(const position_setpoint_s sp)
{
return (PX4_ISFINITE(sp.lat) && PX4_ISFINITE(sp.lon) && PX4_ISFINITE(sp.alt));

1
src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
View File

@ -84,6 +84,7 @@ protected:
matrix::Vector2f _getTargetVelocityXY(); /**< only used for follow-me and only here because of legacy reason.*/
void _updateInternalWaypoints(); /**< Depending on state of vehicle, the internal waypoints might differ from target (for instance if offtrack). */
bool _compute_heading_from_2D_vector(float &heading, matrix::Vector2f v); /**< Computes and sets heading a 2D vector */
void _updateAvoidanceWaypoints(); /**< fill desired_waypoints with the triplets. */
matrix::Vector3f _prev_prev_wp{}; /**< Pre-previous waypoint (local frame). This will be used for smoothing trajectories -> not used yet. */
matrix::Vector3f _prev_wp{}; /**< Previous waypoint (local frame). If no previous triplet is available, the prev_wp is set to current position. */