diff --git a/msg/VehicleOdometry.msg b/msg/VehicleOdometry.msg index 4bd8c5ca30..42718ff44f 100644 --- a/msg/VehicleOdometry.msg +++ b/msg/VehicleOdometry.msg @@ -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 diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 186b7d6478..8a1d34e712 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -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());