ekf2: add simple odometry ENU frame support

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
 

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());