dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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
This commit is contained in:
Daniel Agar 2022-09-28 16:34:14 -04:00
parent 20b46ad33f
commit a27aeb30bf
1 changed files with 18 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
src/modules/mc_pos_control/MulticopterPositionControl.cpp
View File

@ -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)) {