From a27aeb30bf28126b54b091d3809144ea0c360af9 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 28 Sep 2022 16:34:14 -0400 Subject: [PATCH] mc_pos_control: fix trajectory setpoint copy - trajectory_setpoint and vehicle_local_position_setpoint used to be the same data type - we got extremely lucky here that this didn't cause any issues due to all the fields still aligning --- .../MulticopterPositionControl.cpp | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index fa097c7cdf..0151a98aff 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -372,7 +372,24 @@ void MulticopterPositionControl::Run() } } - _trajectory_setpoint_sub.update(&_setpoint); + if (_trajectory_setpoint_sub.updated()) { + trajectory_setpoint_s trajectory_setpoint; + + if (_trajectory_setpoint_sub.copy(&trajectory_setpoint)) { + _setpoint.timestamp = trajectory_setpoint.timestamp; + _setpoint.x = trajectory_setpoint.position[0]; + _setpoint.y = trajectory_setpoint.position[1]; + _setpoint.z = trajectory_setpoint.position[2]; + _setpoint.vx = trajectory_setpoint.velocity[0]; + _setpoint.vy = trajectory_setpoint.velocity[1]; + _setpoint.vz = trajectory_setpoint.velocity[2]; + _setpoint.acceleration[0] = trajectory_setpoint.acceleration[0]; + _setpoint.acceleration[1] = trajectory_setpoint.acceleration[1]; + _setpoint.acceleration[2] = trajectory_setpoint.acceleration[2]; + _setpoint.yaw = trajectory_setpoint.yaw; + _setpoint.yawspeed = trajectory_setpoint.yawspeed; + } + } // adjust existing (or older) setpoint with any EKF reset deltas if ((_setpoint.timestamp != 0) && (_setpoint.timestamp < vehicle_local_position.timestamp)) {