fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-09 01:11:38 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: add simple odometry ENU frame support

Browse Source
This commit is contained in:
Daniel Agar
2022-12-09 14:33:44 -05:00
parent 8cf13d50a8
commit 45d31893e7
2 changed files with 42 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/VehicleOdometry.msg
+11 -9
View File
@@ -3,18 +3,20 @@ uint64 timestamp # time since system start (microseconds)
uint64 timestamp_sample
uint8 POSE_FRAME_UNKNOWN = 0
uint8 POSE_FRAME_NED = 1 # NED earth-fixed frame
uint8 POSE_FRAME_FRD = 2 # FRD world-fixed frame, arbitrary heading reference
uint8 pose_frame # Position and orientation frame of reference
uint8 POSE_FRAME_NED = 1 # NED earth-fixed frame (x: North, y: East, z: Down)
uint8 POSE_FRAME_FRD = 2 # FRD world-fixed frame, arbitrary heading reference (x: Forward, y: Right, z: Down)
uint8 POSE_FRAME_ENU = 3 # ENU earth-fixed frame (x: East, y: North, z: Up)
uint8 pose_frame # Position and orientation frame of reference
float32[3] position # Position in meters. Frame of reference defined by local_frame. NaN if invalid/unknown
float32[4] q # Quaternion rotation from FRD body frame to reference frame. First value NaN if invalid/unknown
float32[3] position # Position in meters. Frame of reference defined by local_frame. NaN if invalid/unknown
float32[4] q # Quaternion rotation from FRD body frame to reference frame. First value NaN if invalid/unknown
uint8 VELOCITY_FRAME_UNKNOWN = 0
uint8 VELOCITY_FRAME_NED = 1 # NED earth-fixed frame
uint8 VELOCITY_FRAME_FRD = 2 # FRD world-fixed frame, arbitrary heading reference
uint8 VELOCITY_FRAME_BODY_FRD = 3 # FRD body-fixed frame
uint8 velocity_frame # Reference frame of the velocity data
uint8 VELOCITY_FRAME_NED = 1 # NED earth-fixed frame (x: North, y: East, z: Down)
uint8 VELOCITY_FRAME_FRD = 2 # FRD world-fixed frame, arbitrary heading reference (x: Forward, y: Right, z: Down)
uint8 VELOCITY_FRAME_BODY_FRD = 3 # FRD body-fixed frame (x: Forward, y: Right, z: Down)
uint8 VELOCITY_FRAME_ENU = 4 # ENU earth-fixed frame (x: East, y: North, z: Up)
uint8 velocity_frame # Reference frame of the velocity data
float32[3] velocity # Velocity in meters/sec. Frame of reference defined by velocity_frame variable. NaN if invalid/unknown
src/modules/ekf2/EKF2.cpp
+31 -4
View File
@@ -1740,10 +1740,26 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo
ev_data.vel_frame = VelocityFrame::BODY_FRAME_FRD;
velocity_frame_valid = true;
break;
case vehicle_odometry_s::VELOCITY_FRAME_ENU:
ev_data.vel_frame = VelocityFrame::LOCAL_FRAME_NED;
velocity_frame_valid = true;
break;
}
if (velocity_frame_valid) {
ev_data.vel = ev_odom_vel;
switch (ev_odom.velocity_frame) {
case vehicle_odometry_s::VELOCITY_FRAME_ENU:
// ENU -> NED
ev_data.vel(0) = ev_odom_vel(0);
ev_data.vel(1) = ev_odom_vel(1);
ev_data.vel(2) = ev_odom_vel(2);
ev_data.vel_frame = VelocityFrame::LOCAL_FRAME_NED;
break;
default:
ev_data.vel = ev_odom_vel;
}
const float evv_noise_var = sq(_param_ekf2_evv_noise.get());
@@ -1781,9 +1797,20 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo
// }
if (position_valid) {
ev_data.pos(0) = ev_odom.position[0];
ev_data.pos(1) = ev_odom.position[1];
ev_data.pos(2) = ev_odom.position[2];
switch (ev_odom.pose_frame) {
case vehicle_odometry_s::POSE_FRAME_ENU:
// ENU -> NED
ev_data.pos(0) = ev_odom.position[1];
ev_data.pos(1) = ev_odom.position[0];
ev_data.pos(2) = -ev_odom.position[2];
break;
default:
ev_data.pos(0) = ev_odom.position[0];
ev_data.pos(1) = ev_odom.position[1];
ev_data.pos(2) = ev_odom.position[2];
break;
}
const float evp_noise_var = sq(_param_ekf2_evp_noise.get());