Skip to content
Snippets Groups Projects
Commit a9bab81e authored by Martina Rivizzigno's avatar Martina Rivizzigno Committed by bresch
Browse files

make format

parent 8c24ba92
No related branches found
No related tags found
No related merge requests found
......@@ -97,7 +97,8 @@ bool FlightTaskAutoMapper2::update()
break;
}
_obstacle_avoidance.prepareAvoidanceSetpoints(_position_setpoint, _velocity_setpoint, _yaw_setpoint, _yawspeed_setpoint);
_obstacle_avoidance.prepareAvoidanceSetpoints(_position_setpoint, _velocity_setpoint, _yaw_setpoint,
_yawspeed_setpoint);
_generateSetpoints();
......
......@@ -52,15 +52,19 @@ ObstacleAvoidance::ObstacleAvoidance(ModuleParams *parent) :
}
void ObstacleAvoidance::prepareAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel_sp, float &yaw_sp, float &yaw_speed_sp) {
void ObstacleAvoidance::prepareAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel_sp, float &yaw_sp,
float &yaw_speed_sp)
{
if (!COM_OBS_AVOID.get()) {
return;
}
_sub_vehicle_trajectory_waypoint.update();
const bool avoidance_data_timeout = hrt_elapsed_time((hrt_abstime *)&_sub_vehicle_trajectory_waypoint.get().timestamp) > TRAJECTORY_STREAM_TIMEOUT_US;
const bool avoidance_point_valid = _sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid == true;
const bool avoidance_data_timeout = hrt_elapsed_time((hrt_abstime *)&_sub_vehicle_trajectory_waypoint.get().timestamp) >
TRAJECTORY_STREAM_TIMEOUT_US;
const bool avoidance_point_valid =
_sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid == true;
if (!avoidance_data_timeout && avoidance_point_valid) {
pos_sp = _sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position;
......
......@@ -53,11 +53,11 @@ public:
ObstacleAvoidance(ModuleParams *parent);
~ObstacleAvoidance() = default;
void prepareAvoidanceSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, float &yaw_sp, float &yaw_speed_sp);
void prepareAvoidanceSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, float &yaw_sp, float &yaw_speed_sp);
private:
uORB::Subscription<vehicle_trajectory_waypoint_s> _sub_vehicle_trajectory_waypoint{ORB_ID(vehicle_trajectory_waypoint)};
uORB::Subscription<vehicle_trajectory_waypoint_s> _sub_vehicle_trajectory_waypoint{ORB_ID(vehicle_trajectory_waypoint)};
DEFINE_PARAMETERS(
(ParamInt<px4::params::COM_OBS_AVOID>) COM_OBS_AVOID /**< obstacle avoidance enabled */
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment