Browse Source

do not update desired setpoints and waypoints if the oa is disabled

sbg
Martina Rivizzigno 6 years ago committed by Matthias Grob
parent
commit
1e8582974c
  1. 9
      src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp
  2. 8
      src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp
  3. 4
      src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
  4. 1
      src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp

9
src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp

@ -86,10 +86,11 @@ bool FlightTaskAutoMapper::update()
_generateVelocitySetpoints(); _generateVelocitySetpoints();
} }
_obstacle_avoidance.updateAvoidanceDesiredSetpoints(_position_setpoint, _velocity_setpoint); if (_param_com_obs_avoid.get()) {
_obstacle_avoidance.updateAvoidanceDesiredSetpoints(_position_setpoint, _velocity_setpoint);
_obstacle_avoidance.injectAvoidanceSetpoints(_position_setpoint, _velocity_setpoint, _yaw_setpoint, _obstacle_avoidance.injectAvoidanceSetpoints(_position_setpoint, _velocity_setpoint, _yaw_setpoint,
_yawspeed_setpoint); _yawspeed_setpoint);
}
// during mission and reposition, raise the landing gears but only // during mission and reposition, raise the landing gears but only
// if altitude is high enough // if altitude is high enough

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

@ -91,10 +91,12 @@ bool FlightTaskAutoMapper2::update()
break; break;
} }
_obstacle_avoidance.updateAvoidanceDesiredSetpoints(_position_setpoint, _velocity_setpoint); if (_param_com_obs_avoid.get()) {
_obstacle_avoidance.updateAvoidanceDesiredSetpoints(_position_setpoint, _velocity_setpoint);
_obstacle_avoidance.injectAvoidanceSetpoints(_position_setpoint, _velocity_setpoint, _yaw_setpoint,
_yawspeed_setpoint);
}
_obstacle_avoidance.injectAvoidanceSetpoints(_position_setpoint, _velocity_setpoint, _yaw_setpoint,
_yawspeed_setpoint);
_generateSetpoints(); _generateSetpoints();

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

@ -89,8 +89,8 @@ void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel
float &yaw_speed_sp) float &yaw_speed_sp)
{ {
if (!COM_OBS_AVOID.get() || _sub_vehicle_status->get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) { if (_sub_vehicle_status->get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) {
// if avoidance disabled or in failsafe nav_state LOITER, don't inject setpoints from avoidance system // if in failsafe nav_state LOITER, don't inject setpoints from avoidance system
return; return;
} }

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

@ -109,7 +109,6 @@ private:
uORB::Subscription<vehicle_status_s> *_sub_vehicle_status{nullptr}; /**< vehicle status subscription */ uORB::Subscription<vehicle_status_s> *_sub_vehicle_status{nullptr}; /**< vehicle status subscription */
DEFINE_PARAMETERS( DEFINE_PARAMETERS(
(ParamInt<px4::params::COM_OBS_AVOID>) COM_OBS_AVOID, /**< obstacle avoidance enabled */
(ParamFloat<px4::params::NAV_MC_ALT_RAD>) NAV_MC_ALT_RAD /**< Acceptance radius for multicopter altitude */ (ParamFloat<px4::params::NAV_MC_ALT_RAD>) NAV_MC_ALT_RAD /**< Acceptance radius for multicopter altitude */
); );

Loading…
Cancel
Save