From e73c50ad9c4613345f7b09652e3bd80f25b38f36 Mon Sep 17 00:00:00 2001 From: Martina Date: Tue, 2 Oct 2018 10:14:16 +0200 Subject: [PATCH] mc_pos_control_main: enable continuos update of vehicle_trajectory_waypoint desired such that it's possible to use it in manual modes --- .../mc_pos_control/mc_pos_control_main.cpp | 28 ++++++++----------- 1 file changed, 12 insertions(+), 16 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index ae8d26cea9..cb29e75f3a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1055,27 +1055,23 @@ MulticopterPositionControl::update_avoidance_waypoint_desired(PositionControlSta vehicle_local_position_setpoint_s &setpoint) { if (MPC_OBS_AVOID.get()) { - const vehicle_trajectory_waypoint_s traj_wp_desired_new = _flight_tasks.getAvoidanceWaypoint(); + _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; - if (traj_wp_desired_new.timestamp > _traj_wp_avoidance_desired.timestamp) { - _traj_wp_avoidance_desired = traj_wp_desired_new; - _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); - 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; - _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); - 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; - _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(); - } + publish_avoidance_desired_waypoint(); } }