From 21f6ca4a1f238271d748b66fb1be637f913cc442 Mon Sep 17 00:00:00 2001
From: Martina Rivizzigno <martina@rivizzigno.it>
Date: Fri, 31 Aug 2018 23:28:15 +0200
Subject: [PATCH] mc_pos_control: fix desired trajectory waypoint. (#10372)
- The first waypoint contains the vehicle current position and the desired velocity setpoint
---
.../mc_pos_control/mc_pos_control_main.cpp | 19 +++++++++++++------
1 file changed, 13 insertions(+), 6 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 4535c46caf..514ed0b176 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -256,7 +256,7 @@ private:
* point2: current triplet only if in auto mode
* @param states current vehicle state
*/
- void update_avoidance_waypoint_desired(PositionControlStates &states);
+ void update_avoidance_waypoint_desired(PositionControlStates &states, vehicle_local_position_setpoint_s &setpoint);
/**
* Check whether or not use the obstacle avoidance waypoint
@@ -635,7 +635,7 @@ MulticopterPositionControl::task_main()
* point_0 contains the current position with the desired velocity
* point_1 contains _pos_sp_triplet.current if valid
*/
- update_avoidance_waypoint_desired(_states);
+ update_avoidance_waypoint_desired(_states, setpoint);
vehicle_constraints_s constraints = _flight_tasks.getConstraints();
@@ -1004,7 +1004,8 @@ MulticopterPositionControl::failsafe(vehicle_local_position_setpoint_s &setpoint
}
void
-MulticopterPositionControl::update_avoidance_waypoint_desired(PositionControlStates &states)
+MulticopterPositionControl::update_avoidance_waypoint_desired(PositionControlStates &states,
+ vehicle_local_position_setpoint_s &setpoint)
{
if (MPC_OBS_AVOID.get()) {
const vehicle_trajectory_waypoint_s traj_wp_desired_new = _flight_tasks.getAvoidanceWaypoint();
@@ -1015,8 +1016,13 @@ MulticopterPositionControl::update_avoidance_waypoint_desired(PositionControlSta
_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].velocity);
- states.position.copyTo(_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].acceleration);
+
+ _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;
@@ -1037,7 +1043,8 @@ MulticopterPositionControl::execute_avoidance_waypoint()
setpoint.vx = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[0];
setpoint.vy = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[1];
- setpoint.vz = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[2];;
+ setpoint.vz = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[2];
+
setpoint.acc_x = setpoint.acc_y = setpoint.acc_z = NAN;
setpoint.yaw = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw;
--
GitLab