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

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

@ -2161,26 +2161,26 @@ 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);
bool velocity_frame_valid = false;
switch (ev_odom.velocity_frame) {
case vehicle_odometry_s::VELOCITY_FRAME_NED:
ev_data.vel_frame = VelocityFrame::LOCAL_FRAME_NED;
velocity_frame_valid = true;
break;
case vehicle_odometry_s::VELOCITY_FRAME_FRD:
ev_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD;
velocity_frame_valid = true;
break;
case vehicle_odometry_s::VELOCITY_FRAME_BODY_FRD:
ev_data.vel_frame = VelocityFrame::BODY_FRAME_FRD;
velocity_frame_valid = true;
break;
}
if (ev_odom_vel.isAllFinite()) {
bool velocity_frame_valid = false;
switch (ev_odom.velocity_frame) {
case vehicle_odometry_s::VELOCITY_FRAME_NED:
ev_data.vel_frame = VelocityFrame::LOCAL_FRAME_NED;
velocity_frame_valid = true;
break;
case vehicle_odometry_s::VELOCITY_FRAME_FRD:
ev_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD;
velocity_frame_valid = true;
break;
case vehicle_odometry_s::VELOCITY_FRAME_BODY_FRD:
ev_data.vel_frame = VelocityFrame::BODY_FRAME_FRD;
velocity_frame_valid = true;
break;
}
if (velocity_frame_valid) {
ev_data.vel = ev_odom_vel;
@ -2205,21 +2205,21 @@ 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);
bool position_frame_valid = false;
switch (ev_odom.pose_frame) {
case vehicle_odometry_s::POSE_FRAME_NED:
ev_data.pos_frame = PositionFrame::LOCAL_FRAME_NED;
position_frame_valid = true;
break;
case vehicle_odometry_s::POSE_FRAME_FRD:
ev_data.pos_frame = PositionFrame::LOCAL_FRAME_FRD;
position_frame_valid = true;
break;
}
if (ev_odom_pos.isAllFinite()) {
bool position_frame_valid = false;
switch (ev_odom.pose_frame) {
case vehicle_odometry_s::POSE_FRAME_NED:
ev_data.pos_frame = PositionFrame::LOCAL_FRAME_NED;
position_frame_valid = true;
break;
case vehicle_odometry_s::POSE_FRAME_FRD:
ev_data.pos_frame = PositionFrame::LOCAL_FRAME_FRD;
position_frame_valid = true;
break;
}
if (position_frame_valid) {
ev_data.pos = ev_odom_pos;