fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

mecanum: update current position in main file

Browse Source
This commit is contained in:
chfriedrich98 2024-11-26 16:45:34 +01:00 committed by chfriedrich98
parent a1b68fcac2
commit ff55313b0b
1 changed files with 1 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/modules/rover_mecanum/RoverMecanum.cpp
View File

@ -283,6 +283,7 @@ void RoverMecanum::updateSubscriptions()
if (_vehicle_local_position_sub.updated()) {
vehicle_local_position_s vehicle_local_position{};
_vehicle_local_position_sub.copy(&vehicle_local_position);
_curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
// Apply threshold to the velocity measurement to cut off measurement noise when standing still