72 const bool avoidance_point_valid =
78 PX4_WARN(
"Obstacle Avoidance system failed, loitering");
97 if (avoidance_point_valid) {
111 const float curr_yawspeed,
const Vector3f &next_wp,
const float next_yaw,
const float next_yawspeed,
112 const bool ext_yaw_active,
const int wp_type)
150 float target_acceptance_radius,
const Vector2f &closest_pt)
161 const float prev_curr_travelled = prev_to_closest_pt.
length() / prev_to_target.
length();
165 if (prev_curr_travelled > 1.0
f) {
176 if (pos_to_target.
length() < target_acceptance_radius && pos_to_target_z > _param_nav_mc_alt_rad.get()
194 command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
void updateAvoidanceDesiredSetpoints(const matrix::Vector3f &pos_sp, const matrix::Vector3f &vel_sp, const int type)
Updates the desired setpoints to send to the obstacle avoidance system.
void checkAvoidanceProgress(const matrix::Vector3f &pos, const matrix::Vector3f &prev_wp, float target_acceptance_radius, const matrix::Vector2f &closest_pt)
Checks the vehicle progress between previous and current position waypoint of the triplet...
bool publish(const T &data)
Publish the struct.
systemlib::Hysteresis _no_progress_z_hysteresis
becomes true if the vehicle is not making progress towards the z component of the goal ...
ObstacleAvoidance(ModuleParams *parent)
vehicle_trajectory_waypoint_s _desired_waypoint
desired vehicle trajectory waypoint to be sent to OA
void injectAvoidanceSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, float &yaw_sp, float &yaw_speed_sp)
Inject setpoints from obstacle avoidance system into FlightTasks.
static constexpr uint64_t TIME_BEFORE_FAILSAFE
If Flighttask fails, keep 0.5 seconds the current setpoint before going into failsafe land...
matrix::Vector3f _failsafe_position
vehicle position when entered in failsafe
void copyTo(Type dst[M *N]) const
float altitude_acceptance
float _prev_pos_to_target_z
z distance to the goal
matrix::Vector3f _curr_wp
current position triplet
void set_state_and_update(const bool new_state, const hrt_abstime &now_us)
bool publish(const T &data)
Publish the struct.
systemlib::Hysteresis _avoidance_point_not_valid_hysteresis
becomes true if the companion doesn't start sending valid setpoints
const vehicle_trajectory_waypoint_s empty_trajectory_waypoint
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
uORB::SubscriptionData< vehicle_status_s > _sub_vehicle_status
vehicle status subscription
Vector2< float > Vector2f
static hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
Compute the delta between a timestamp taken in the past and now.
static constexpr uint64_t Z_PROGRESS_TIMEOUT_US
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Vector3< float > Vector3f
static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US
Timeout in us for trajectory data to get considered invalid.
void set_hysteresis_time_from(const bool from_state, const hrt_abstime new_hysteresis_time_us)
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, const bool ext_yaw_active, const int wp_type)
Updates the desired waypoints to send to the obstacle avoidance system.
uORB::SubscriptionData< vehicle_trajectory_waypoint_s > _sub_vehicle_trajectory_waypoint
vehicle trajectory waypoint subscription
matrix::Vector3f _position
current vehicle position
uORB::Publication< vehicle_trajectory_waypoint_s > _pub_traj_wp_avoidance_desired
trajectory waypoint desired publication
uORB::Publication< position_controller_status_s > _pub_pos_control_status
position controller status publication
struct trajectory_waypoint_s waypoints[5]
uORB::PublicationQueued< vehicle_command_s > _pub_vehicle_command
vehicle command do publication
void _publishVehicleCmdDoLoiter()
Publishes vehicle command.
bool _ext_yaw_active
true, if external yaw handling is active
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
This class is used to inject the setpoints of an obstacle avoidance system into the FlightTasks...