Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-30 17:40:12 +08:00
Code Issues Actions 4 Packages Projects Releases Wiki Activity

ev_odom: always convert reference frame enum

Browse Source
This commit is contained in:
GuillaumeLaine
2024-01-18 15:48:51 +01:00
committed by Beat Küng
parent 6fcfd5fac1
commit 648e730c4a
1 changed files with 33 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+2 -2
View File
@@ -2197,7 +2197,6 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps)
const Vector3f ev_odom_vel(ev_odom.velocity); const Vector3f ev_odom_vel(ev_odom.velocity);
const Vector3f ev_odom_vel_var(ev_odom.velocity_variance); const Vector3f ev_odom_vel_var(ev_odom.velocity_variance);
if (ev_odom_vel.isAllFinite()) {
bool velocity_frame_valid = false; bool velocity_frame_valid = false;
switch (ev_odom.velocity_frame) { switch (ev_odom.velocity_frame) {
@@ -2217,6 +2216,7 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps)
break; break;
} }
if (ev_odom_vel.isAllFinite()) {
if (velocity_frame_valid) { if (velocity_frame_valid) {
ev_data.vel = ev_odom_vel; ev_data.vel = ev_odom_vel;
@@ -2241,7 +2241,6 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps)
const Vector3f ev_odom_pos(ev_odom.position); const Vector3f ev_odom_pos(ev_odom.position);
const Vector3f ev_odom_pos_var(ev_odom.position_variance); const Vector3f ev_odom_pos_var(ev_odom.position_variance);
if (ev_odom_pos.isAllFinite()) {
bool position_frame_valid = false; bool position_frame_valid = false;
switch (ev_odom.pose_frame) { switch (ev_odom.pose_frame) {
@@ -2256,6 +2255,7 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps)
break; break;
} }
if (ev_odom_pos.isAllFinite()) {
if (position_frame_valid) { if (position_frame_valid) {
ev_data.pos = ev_odom_pos; ev_data.pos = ev_odom_pos;