Browse Source

make format

sbg
Martina Rivizzigno 6 years ago committed by bresch
parent
commit
a9bab81eb8
  1. 3
      src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp
  2. 10
      src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
  3. 4
      src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp

3
src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp

@ -97,7 +97,8 @@ bool FlightTaskAutoMapper2::update() @@ -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();

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

@ -52,15 +52,19 @@ ObstacleAvoidance::ObstacleAvoidance(ModuleParams *parent) : @@ -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;

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

@ -53,11 +53,11 @@ public: @@ -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 */

Loading…
Cancel
Save