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

ev_odom: always convert reference frame enum

This commit is contained in:
GuillaumeLaine 2024-01-18 15:48:51 +01:00 committed by Beat Küng
parent b32d1c295b
commit d120b8b6c0
No known key found for this signature in database
GPG Key ID: 866DB5F0E24821BB
1 changed files with 33 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/ekf2/EKF2.cpp
View File

@ -2161,7 +2161,6 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps)
const Vector3f ev_odom_vel(ev_odom.velocity);
const Vector3f ev_odom_vel_var(ev_odom.velocity_variance);
if (ev_odom_vel.isAllFinite()) {
bool velocity_frame_valid = false;
switch (ev_odom.velocity_frame) {
@ -2181,6 +2180,7 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps)
break;
}
if (ev_odom_vel.isAllFinite()) {
if (velocity_frame_valid) {
ev_data.vel = ev_odom_vel;
@ -2205,7 +2205,6 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps)
const Vector3f ev_odom_pos(ev_odom.position);
const Vector3f ev_odom_pos_var(ev_odom.position_variance);
if (ev_odom_pos.isAllFinite()) {
bool position_frame_valid = false;
switch (ev_odom.pose_frame) {
@ -2220,6 +2219,7 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps)
break;
}
if (ev_odom_pos.isAllFinite()) {
if (position_frame_valid) {
ev_data.pos = ev_odom_pos;