fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 19:43:35 +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-08-09 16:38:47 -04:00
committed by Daniel Agar
parent 7c7602873e
commit 17a08a9de7
2 changed files with 29 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
+28
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));