Browse Source

rename updateAvoidanceWaypoints->updateAvoidanceDesiredWaypoints and

updateAvoidanceSetpoints->updateAvoidanceDesiredSetpoints
sbg
Martina Rivizzigno 6 years ago committed by bresch
parent
commit
c1258931da
  1. 5
      src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
  2. 4
      src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
  3. 6
      src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp

5
src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp

@ -243,10 +243,11 @@ bool FlightTaskAuto::_evaluateTriplets() @@ -243,10 +243,11 @@ bool FlightTaskAuto::_evaluateTriplets()
}
if (_param_com_obs_avoid.get() && _sub_vehicle_status->get().is_rotary_wing) {
_obstacle_avoidance.updateAvoidanceWaypoints(_triplet_target, _yaw_setpoint, _yawspeed_setpoint, _triplet_next_wp,
_obstacle_avoidance.updateAvoidanceDesiredWaypoints(_triplet_target, _yaw_setpoint, _yawspeed_setpoint,
_triplet_next_wp,
_sub_triplet_setpoint->get().next.yaw,
_sub_triplet_setpoint->get().next.yawspeed_valid ? _sub_triplet_setpoint->get().next.yawspeed : NAN);
_obstacle_avoidance.updateAvoidanceSetpoints(_position_setpoint, _velocity_setpoint);
_obstacle_avoidance.updateAvoidanceDesiredSetpoints(_position_setpoint, _velocity_setpoint);
_obstacle_avoidance.checkAvoidanceProgress(_position, _triplet_prev_wp, _target_acceptance_radius, _closest_pt);
}

4
src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp

@ -110,7 +110,7 @@ void ObstacleAvoidance::prepareAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &ve @@ -110,7 +110,7 @@ void ObstacleAvoidance::prepareAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &ve
yaw_speed_sp = _sub_vehicle_trajectory_waypoint->get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw_speed;
}
void ObstacleAvoidance::updateAvoidanceWaypoints(const Vector3f &curr_wp, const float curr_yaw,
void ObstacleAvoidance::updateAvoidanceDesiredWaypoints(const Vector3f &curr_wp, const float curr_yaw,
const float curr_yawspeed,
const Vector3f &next_wp, const float next_yaw, const float next_yawspeed)
{
@ -135,7 +135,7 @@ void ObstacleAvoidance::updateAvoidanceWaypoints(const Vector3f &curr_wp, const @@ -135,7 +135,7 @@ void ObstacleAvoidance::updateAvoidanceWaypoints(const Vector3f &curr_wp, const
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].point_valid = true;
}
void ObstacleAvoidance::updateAvoidanceSetpoints(const Vector3f &pos_sp, const Vector3f &vel_sp)
void ObstacleAvoidance::updateAvoidanceDesiredSetpoints(const Vector3f &pos_sp, const Vector3f &vel_sp)
{
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);

6
src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp

@ -81,14 +81,14 @@ public: @@ -81,14 +81,14 @@ public:
* @param next_yaw, next yaw triplet
* @param next_yawspeed, next yaw speed triplet
*/
void updateAvoidanceWaypoints(const matrix::Vector3f &curr_wp, const float curr_yaw, const float curr_yawspeed,
const matrix::Vector3f &next_wp, const float next_yaw, const float next_yawspeed);
void updateAvoidanceDesiredWaypoints(const matrix::Vector3f &curr_wp, const float curr_yaw, const float curr_yawspeed,
const matrix::Vector3f &next_wp, const float next_yaw, const float next_yawspeed);
/**
* Updates the desired setpoints to send to the obstacle avoidance system.
* @param pos_sp, desired position setpoint computed by the active FlightTask
* @param vel_sp, desired velocity setpoint computed by the active FlightTask
*/
void updateAvoidanceSetpoints(const matrix::Vector3f &pos_sp, const matrix::Vector3f &vel_sp);
void updateAvoidanceDesiredSetpoints(const matrix::Vector3f &pos_sp, const matrix::Vector3f &vel_sp);
/**
* Checks the vehicle progress between previous and current position triplet.

Loading…
Cancel
Save