Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-26 09:26:25 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

FlightTaskAuto: use the triplets from navigator and not with the internal

Browse Source 
ones for obstacle avoidance. Otherwise the vehicle is continuolsy in the
offtrack state. Use already comnputed yaw and yaw speed setpoints instead
of subscription
This commit is contained in:
Martina
2018-08-06 09:34:52 +02:00
committed by Daniel Agar
parent 8096e84133
commit 2d9bbeb7ed
1 changed files with 4 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
+4 -6
View File
@@ -269,18 +269,16 @@ void FlightTaskAuto::_updateAvoidanceWaypoints()
{
_desired_waypoint.timestamp = hrt_absolute_time();
_target.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].position);
_triplet_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].yaw = _yaw_setpoint;
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw_speed = _yawspeed_setpoint;
_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);
_triplet_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);