forked from Archive/PX4-Autopilot
move all obstacle avoidance interfaces to the ObstacleAvoidance library
This commit is contained in:
Martina Rivizzigno
bresch
parent
4e806d79fe
commit
34b0f33098
|
|
@ -235,14 +235,20 @@ bool FlightTaskAuto::_evaluateTriplets()
|
|||
State previous_state = _current_state;
|
||||
_current_state = _getCurrentState();
|
||||
|
||||
|
||||
|
||||
if (triplet_update || (_current_state != previous_state)) {
|
||||
_updateInternalWaypoints();
|
||||
_updateAvoidanceWaypoints();
|
||||
_mission_gear = _sub_triplet_setpoint->get().current.landing_gear;
|
||||
}
|
||||
|
||||
if (_param_com_obs_avoid.get() && _sub_vehicle_status->get().is_rotary_wing) {
|
||||
_checkAvoidanceProgress();
|
||||
_obstacle_avoidance.updateAvoidanceWaypoints(_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.checkAvoidanceProgress(_position, _triplet_prev_wp, _target_acceptance_radius, _closest_pt);
|
||||
// _checkAvoidanceProgress();
|
||||
}
|
||||
|
||||
return true;
|
||||
|
|
@ -301,70 +307,6 @@ void FlightTaskAuto::_set_heading_from_mode()
|
|||
}
|
||||
}
|
||||
|
||||
void FlightTaskAuto::_updateAvoidanceWaypoints()
|
||||
{
|
||||
_desired_waypoint.timestamp = hrt_absolute_time();
|
||||
|
||||
_triplet_target.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].position);
|
||||
Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].velocity);
|
||||
Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].acceleration);
|
||||
|
||||
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw = _yaw_setpoint;
|
||||
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw_speed = _yawspeed_setpoint;
|
||||
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].point_valid = true;
|
||||
|
||||
|
||||
_triplet_next_wp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].position);
|
||||
Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].velocity);
|
||||
Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].acceleration);
|
||||
|
||||
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw = _sub_triplet_setpoint->get().next.yaw;
|
||||
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw_speed =
|
||||
_sub_triplet_setpoint->get().next.yawspeed_valid ?
|
||||
_sub_triplet_setpoint->get().next.yawspeed : NAN;
|
||||
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].point_valid = true;
|
||||
}
|
||||
|
||||
void FlightTaskAuto::_checkAvoidanceProgress()
|
||||
{
|
||||
position_controller_status_s pos_control_status = {};
|
||||
pos_control_status.timestamp = hrt_absolute_time();
|
||||
|
||||
// vector from previous triplet to current target
|
||||
Vector2f prev_to_target = Vector2f(_triplet_target - _triplet_prev_wp);
|
||||
// vector from previous triplet to the vehicle projected position on the line previous-target triplet
|
||||
Vector2f prev_to_closest_pt = _closest_pt - Vector2f(_triplet_prev_wp);
|
||||
// fraction of the previous-tagerget line that has been flown
|
||||
const float prev_curr_travelled = prev_to_closest_pt.length() / prev_to_target.length();
|
||||
|
||||
Vector2f pos_to_target = Vector2f(_triplet_target - _position);
|
||||
|
||||
if (prev_curr_travelled > 1.0f) {
|
||||
// if the vehicle projected position on the line previous-target is past the target waypoint,
|
||||
// increase the target acceptance radius such that navigator will update the triplets
|
||||
pos_control_status.acceptance_radius = pos_to_target.length() + 0.5f;
|
||||
}
|
||||
|
||||
const float pos_to_target_z = fabsf(_triplet_target(2) - _position(2));
|
||||
|
||||
if (pos_to_target.length() < _target_acceptance_radius && pos_to_target_z > _param_nav_mc_alt_rad.get()) {
|
||||
// vehicle above or below the target waypoint
|
||||
pos_control_status.altitude_acceptance = pos_to_target_z + 0.5f;
|
||||
}
|
||||
|
||||
// do not check for waypoints yaw acceptance in navigator
|
||||
pos_control_status.yaw_acceptance = NAN;
|
||||
|
||||
if (_pub_pos_control_status == nullptr) {
|
||||
_pub_pos_control_status = orb_advertise(ORB_ID(position_controller_status), &pos_control_status);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(position_controller_status), _pub_pos_control_status, &pos_control_status);
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
bool FlightTaskAuto::_isFinite(const position_setpoint_s &sp)
|
||||
{
|
||||
return (PX4_ISFINITE(sp.lat) && PX4_ISFINITE(sp.lon) && PX4_ISFINITE(sp.alt));
|
||||
|
|
|
|||
|
|
@ -43,7 +43,6 @@
|
|||
#include <uORB/topics/position_setpoint_triplet.h>
|
||||
#include <uORB/topics/position_setpoint.h>
|
||||
#include <uORB/topics/home_position.h>
|
||||
#include <uORB/topics/position_controller_status.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <lib/ecl/geo/geo.h>
|
||||
#include "lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp"
|
||||
|
|
@ -93,7 +92,6 @@ protected:
|
|||
matrix::Vector2f _getTargetVelocityXY(); /**< only used for follow-me and only here because of legacy reason.*/
|
||||
void _updateInternalWaypoints(); /**< Depending on state of vehicle, the internal waypoints might differ from target (for instance if offtrack). */
|
||||
bool _compute_heading_from_2D_vector(float &heading, matrix::Vector2f v); /**< Computes and sets heading a 2D vector */
|
||||
void _updateAvoidanceWaypoints(); /**< fill desired_waypoints with the triplets. */
|
||||
|
||||
matrix::Vector3f _prev_prev_wp{}; /**< Pre-previous waypoint (local frame). This will be used for smoothing trajectories -> not used yet. */
|
||||
matrix::Vector3f _prev_wp{}; /**< Previous waypoint (local frame). If no previous triplet is available, the prev_wp is set to current position. */
|
||||
|
|
@ -139,12 +137,10 @@ private:
|
|||
WeatherVane *_ext_yaw_handler =
|
||||
nullptr; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
|
||||
|
||||
orb_advert_t _pub_pos_control_status = nullptr; /**< Publisher for the position controller status */
|
||||
|
||||
bool _evaluateTriplets(); /**< Checks and sets triplets. */
|
||||
bool _isFinite(const position_setpoint_s &sp); /**< Checks if all waypoint triplets are finite. */
|
||||
bool _evaluateGlobalReference(); /**< Check is global reference is available. */
|
||||
State _getCurrentState(); /**< Computes the current vehicle state based on the vehicle position and navigator triplets. */
|
||||
void _set_heading_from_mode(); /**< @see MPC_YAW_MODE */
|
||||
void _checkAvoidanceProgress();
|
||||
};
|
||||
|
|
|
|||
|
|
@ -52,6 +52,19 @@ ObstacleAvoidance::ObstacleAvoidance(ModuleParams *parent) :
|
|||
|
||||
}
|
||||
|
||||
ObstacleAvoidance::~ObstacleAvoidance()
|
||||
{
|
||||
//unadvertise publishers
|
||||
if (_traj_wp_avoidance_desired_pub != nullptr) {
|
||||
orb_unadvertise(_traj_wp_avoidance_desired_pub);
|
||||
}
|
||||
|
||||
if (_pub_pos_control_status != nullptr) {
|
||||
orb_unadvertise(_pub_pos_control_status);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
bool ObstacleAvoidance::initializeSubscriptions(SubscriptionArray &subscription_array)
|
||||
{
|
||||
if (!subscription_array.get(ORB_ID(vehicle_trajectory_waypoint), _sub_vehicle_trajectory_waypoint)) {
|
||||
|
|
@ -81,3 +94,93 @@ 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,
|
||||
const float curr_yawspeed,
|
||||
const Vector3f &next_wp, const float next_yaw, const float next_yawspeed)
|
||||
{
|
||||
_desired_waypoint.timestamp = hrt_absolute_time();
|
||||
_desired_waypoint.type = vehicle_trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS;
|
||||
|
||||
curr_wp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].position);
|
||||
Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].velocity);
|
||||
Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].acceleration);
|
||||
|
||||
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw = curr_yaw;
|
||||
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw_speed = curr_yawspeed;
|
||||
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].point_valid = true;
|
||||
|
||||
next_wp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].position);
|
||||
Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].velocity);
|
||||
Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].acceleration);
|
||||
|
||||
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw = next_yaw;
|
||||
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw_speed = next_yawspeed;
|
||||
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].point_valid = true;
|
||||
}
|
||||
|
||||
void ObstacleAvoidance::updateAvoidanceSetpoints(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);
|
||||
|
||||
_publish_avoidance_desired_waypoint();
|
||||
|
||||
// TODO: reset all fields to NaN
|
||||
}
|
||||
|
||||
void ObstacleAvoidance::checkAvoidanceProgress(const Vector3f &pos, const Vector3f &prev_wp,
|
||||
float target_acceptance_radius,
|
||||
const Vector2f &closest_pt)
|
||||
{
|
||||
position_controller_status_s pos_control_status = {};
|
||||
pos_control_status.timestamp = hrt_absolute_time();
|
||||
Vector3f curr_wp = _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].position;
|
||||
|
||||
// vector from previous triplet to current target
|
||||
Vector2f prev_to_target = Vector2f(curr_wp - prev_wp);
|
||||
// vector from previous triplet to the vehicle projected position on the line previous-target triplet
|
||||
Vector2f prev_to_closest_pt = closest_pt - Vector2f(prev_wp);
|
||||
// fraction of the previous-tagerget line that has been flown
|
||||
const float prev_curr_travelled = prev_to_closest_pt.length() / prev_to_target.length();
|
||||
|
||||
Vector2f pos_to_target = Vector2f(curr_wp - pos);
|
||||
|
||||
if (prev_curr_travelled > 1.0f) {
|
||||
// if the vehicle projected position on the line previous-target is past the target waypoint,
|
||||
// increase the target acceptance radius such that navigator will update the triplets
|
||||
pos_control_status.acceptance_radius = pos_to_target.length() + 0.5f;
|
||||
}
|
||||
|
||||
const float pos_to_target_z = fabsf(curr_wp(2) - pos(2));
|
||||
|
||||
if (pos_to_target.length() < target_acceptance_radius && pos_to_target_z > NAV_MC_ALT_RAD.get()) {
|
||||
// vehicle above or below the target waypoint
|
||||
pos_control_status.altitude_acceptance = pos_to_target_z + 0.5f;
|
||||
}
|
||||
|
||||
// do not check for waypoints yaw acceptance in navigator
|
||||
pos_control_status.yaw_acceptance = NAN;
|
||||
|
||||
if (_pub_pos_control_status == nullptr) {
|
||||
_pub_pos_control_status = orb_advertise(ORB_ID(position_controller_status), &pos_control_status);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(position_controller_status), _pub_pos_control_status, &pos_control_status);
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void
|
||||
ObstacleAvoidance::_publish_avoidance_desired_waypoint()
|
||||
{
|
||||
// publish desired waypoint
|
||||
if (_traj_wp_avoidance_desired_pub != nullptr) {
|
||||
orb_publish(ORB_ID(vehicle_trajectory_waypoint_desired), _traj_wp_avoidance_desired_pub, &_desired_waypoint);
|
||||
|
||||
} else {
|
||||
_traj_wp_avoidance_desired_pub = orb_advertise(ORB_ID(vehicle_trajectory_waypoint_desired),
|
||||
&_desired_waypoint);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -43,6 +43,7 @@
|
|||
|
||||
#include <px4_module_params.h>
|
||||
#include <uORB/topics/vehicle_trajectory_waypoint.h>
|
||||
#include <uORB/topics/position_controller_status.h>
|
||||
#include <lib/FlightTasks/tasks/FlightTask/SubscriptionArray.hpp>
|
||||
#include <matrix/matrix/math.hpp>
|
||||
#include <px4_defines.h>
|
||||
|
|
@ -51,18 +52,31 @@ class ObstacleAvoidance : public ModuleParams
|
|||
{
|
||||
public:
|
||||
ObstacleAvoidance(ModuleParams *parent);
|
||||
~ObstacleAvoidance() = default;
|
||||
~ObstacleAvoidance();
|
||||
|
||||
bool initializeSubscriptions(SubscriptionArray &subscription_array);
|
||||
|
||||
void prepareAvoidanceSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, float &yaw_sp, float &yaw_speed_sp);
|
||||
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 updateAvoidanceSetpoints(const matrix::Vector3f &pos_sp, const matrix::Vector3f &vel_sp);
|
||||
void checkAvoidanceProgress(const matrix::Vector3f &pos, const matrix::Vector3f &prev_wp,
|
||||
float target_acceptance_radius,
|
||||
const matrix::Vector2f &closest_pt);
|
||||
|
||||
private:
|
||||
|
||||
uORB::Subscription<vehicle_trajectory_waypoint_s> *_sub_vehicle_trajectory_waypoint{nullptr};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamInt<px4::params::COM_OBS_AVOID>) COM_OBS_AVOID /**< obstacle avoidance enabled */
|
||||
(ParamInt<px4::params::COM_OBS_AVOID>) COM_OBS_AVOID, /**< obstacle avoidance enabled */
|
||||
(ParamFloat<px4::params::NAV_MC_ALT_RAD>) NAV_MC_ALT_RAD
|
||||
);
|
||||
|
||||
vehicle_trajectory_waypoint_s _desired_waypoint = {};
|
||||
orb_advert_t _traj_wp_avoidance_desired_pub{nullptr}; /**< trajectory waypoint desired publication */
|
||||
orb_advert_t _pub_pos_control_status{nullptr};
|
||||
|
||||
void _publish_avoidance_desired_waypoint();
|
||||
|
||||
};
|
||||
|
|
|
|||
|
|
@ -108,7 +108,6 @@ private:
|
|||
orb_advert_t _att_sp_pub{nullptr}; /**< attitude setpoint publication */
|
||||
orb_advert_t _traj_sp_pub{nullptr}; /**< trajectory setpoints publication */
|
||||
orb_advert_t _local_pos_sp_pub{nullptr}; /**< vehicle local position setpoint publication */
|
||||
orb_advert_t _traj_wp_avoidance_desired_pub{nullptr}; /**< trajectory waypoint desired publication */
|
||||
orb_advert_t _pub_vehicle_command{nullptr}; /**< vehicle command publication */
|
||||
orb_id_t _attitude_setpoint_id{nullptr};
|
||||
orb_advert_t _landing_gear_pub{nullptr};
|
||||
|
|
@ -120,7 +119,6 @@ private:
|
|||
int _local_pos_sub{-1}; /**< vehicle local position */
|
||||
int _att_sub{-1}; /**< vehicle attitude */
|
||||
int _home_pos_sub{-1}; /**< home position */
|
||||
int _traj_wp_avoidance_sub{-1}; /**< trajectory waypoint */
|
||||
|
||||
int _task_failure_count{0}; /**< counter for task failures */
|
||||
|
||||
|
|
@ -144,8 +142,6 @@ private:
|
|||
vehicle_control_mode_s _control_mode{}; /**< vehicle control mode */
|
||||
vehicle_local_position_s _local_pos{}; /**< vehicle local position */
|
||||
home_position_s _home_pos{}; /**< home position */
|
||||
vehicle_trajectory_waypoint_s _traj_wp_avoidance{}; /**< trajectory waypoint */
|
||||
vehicle_trajectory_waypoint_s _traj_wp_avoidance_desired{}; /**< desired waypoints, inputs to an obstacle avoidance module */
|
||||
landing_gear_s _landing_gear{};
|
||||
int8_t _old_landing_gear_position{landing_gear_s::GEAR_KEEP};
|
||||
|
||||
|
|
@ -160,7 +156,6 @@ private:
|
|||
(ParamInt<px4::params::MPC_AUTO_MODE>) _param_mpc_auto_mode,
|
||||
(ParamInt<px4::params::MPC_ALT_MODE>) _param_mpc_alt_mode,
|
||||
(ParamFloat<px4::params::MPC_SPOOLUP_TIME>) _param_mpc_spoolup_time, /**< time to let motors spool up after arming */
|
||||
(ParamInt<px4::params::COM_OBS_AVOID>) _param_com_obs_avoid, /**< enable obstacle avoidance */
|
||||
(ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd /**< maximum tilt for landing and smooth takeoff */
|
||||
);
|
||||
|
||||
|
|
@ -275,24 +270,11 @@ private:
|
|||
void failsafe(vehicle_local_position_setpoint_s &setpoint, const PositionControlStates &states, const bool force,
|
||||
const bool warn);
|
||||
|
||||
/**
|
||||
* Fill desired vehicle_trajectory_waypoint:
|
||||
* point1: current position, desired velocity
|
||||
* point2: current triplet only if in auto mode
|
||||
* @param states current vehicle state
|
||||
*/
|
||||
void update_avoidance_waypoint_desired(PositionControlStates &states, vehicle_local_position_setpoint_s &setpoint);
|
||||
|
||||
/**
|
||||
* Reset setpoints to NAN
|
||||
*/
|
||||
void reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint);
|
||||
|
||||
/**
|
||||
* Publish desired vehicle_trajectory_waypoint
|
||||
*/
|
||||
void publish_avoidance_desired_waypoint();
|
||||
|
||||
/**
|
||||
* Shim for calling task_main from task_create.
|
||||
*/
|
||||
|
|
@ -461,11 +443,6 @@ MulticopterPositionControl::poll_subscriptions()
|
|||
orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos);
|
||||
}
|
||||
|
||||
orb_check(_traj_wp_avoidance_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(vehicle_trajectory_waypoint), _traj_wp_avoidance_sub, &_traj_wp_avoidance);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
|
|
@ -574,7 +551,6 @@ MulticopterPositionControl::run()
|
|||
_params_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
||||
_home_pos_sub = orb_subscribe(ORB_ID(home_position));
|
||||
_traj_wp_avoidance_sub = orb_subscribe(ORB_ID(vehicle_trajectory_waypoint));
|
||||
|
||||
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
|
||||
orb_set_interval(_local_pos_sub, 20); // 50 Hz updates
|
||||
|
|
@ -671,12 +647,6 @@ MulticopterPositionControl::run()
|
|||
|
||||
publish_trajectory_sp(setpoint);
|
||||
|
||||
/* desired waypoints for obstacle avoidance:
|
||||
* point_0 contains the current position with the desired velocity
|
||||
* point_1 contains _pos_sp_triplet.current if valid
|
||||
*/
|
||||
update_avoidance_waypoint_desired(_states, setpoint);
|
||||
|
||||
vehicle_constraints_s constraints = _flight_tasks.getConstraints();
|
||||
landing_gear_s gear = _flight_tasks.getGear();
|
||||
|
||||
|
|
@ -826,7 +796,6 @@ MulticopterPositionControl::run()
|
|||
orb_unsubscribe(_local_pos_sub);
|
||||
orb_unsubscribe(_att_sub);
|
||||
orb_unsubscribe(_home_pos_sub);
|
||||
orb_unsubscribe(_traj_wp_avoidance_sub);
|
||||
}
|
||||
|
||||
void
|
||||
|
|
@ -1128,31 +1097,6 @@ MulticopterPositionControl::failsafe(vehicle_local_position_setpoint_s &setpoint
|
|||
}
|
||||
}
|
||||
|
||||
void
|
||||
MulticopterPositionControl::update_avoidance_waypoint_desired(PositionControlStates &states,
|
||||
vehicle_local_position_setpoint_s &setpoint)
|
||||
{
|
||||
if (_param_com_obs_avoid.get()) {
|
||||
_traj_wp_avoidance_desired = _flight_tasks.getAvoidanceWaypoint();
|
||||
_traj_wp_avoidance_desired.timestamp = hrt_absolute_time();
|
||||
_traj_wp_avoidance_desired.type = vehicle_trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS;
|
||||
|
||||
states.position.copyTo(_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position);
|
||||
|
||||
_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[0] = setpoint.vx;
|
||||
_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[1] = setpoint.vy;
|
||||
_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[2] = setpoint.vz;
|
||||
|
||||
states.acceleration.copyTo(_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].acceleration);
|
||||
|
||||
_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw = states.yaw;
|
||||
_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw_speed = NAN;
|
||||
_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid = true;
|
||||
|
||||
publish_avoidance_desired_waypoint();
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
MulticopterPositionControl::reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint)
|
||||
{
|
||||
|
|
@ -1163,19 +1107,6 @@ MulticopterPositionControl::reset_setpoint_to_nan(vehicle_local_position_setpoin
|
|||
setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = NAN;
|
||||
}
|
||||
|
||||
void
|
||||
MulticopterPositionControl::publish_avoidance_desired_waypoint()
|
||||
{
|
||||
// publish desired waypoint
|
||||
if (_traj_wp_avoidance_desired_pub != nullptr) {
|
||||
orb_publish(ORB_ID(vehicle_trajectory_waypoint_desired), _traj_wp_avoidance_desired_pub, &_traj_wp_avoidance_desired);
|
||||
|
||||
} else {
|
||||
_traj_wp_avoidance_desired_pub = orb_advertise(ORB_ID(vehicle_trajectory_waypoint_desired),
|
||||
&_traj_wp_avoidance_desired);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
MulticopterPositionControl::publish_attitude()
|
||||
{
|
||||
|
|
|
|||
Loading…
Reference in New Issue