SITL: support simulation of ODOMETRY message

libraries/SITL/SIM_Vicon.cpp

@@ -207,6 +207,33 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
         msg_buf[msg_buf_index].time_send_us = time_send_us;
     }
 
+
+    // send ODOMETRY message
+    if (should_send(ViconTypeMask::ODOMETRY) && get_free_msg_buf_index(msg_buf_index)) {
+        mavlink_msg_odometry_pack_chan(
+            system_id,
+            component_id,
+            mavlink_ch,
+            &msg_buf[msg_buf_index].obs_msg,
+            now_us + time_offset_us,
+            MAV_FRAME_LOCAL_FRD,
+            MAV_FRAME_BODY_FRD,
+            pos_corrected.x,
+            pos_corrected.y,
+            pos_corrected.z,
+            &attitude[0],
+            vel_corrected.x,
+            vel_corrected.y,
+            vel_corrected.z,
+            gyro.x,
+            gyro.y,
+            gyro.z,
+            NULL, NULL,
+            0,
+            MAV_ESTIMATOR_TYPE_VIO);
+        msg_buf[msg_buf_index].time_send_us = time_send_us;
+    }
+
     // determine time, position, and angular deltas
     uint64_t time_delta = now_us - last_observation_usec;
 

libraries/SITL/SIM_Vicon.h

@@ -60,7 +60,8 @@ private:
         VISION_POSITION_ESTIMATE    = (1 << 0),
         VISION_SPEED_ESTIMATE       = (1 << 1),
         VICON_POSITION_ESTIMATE     = (1 << 2),
-        VISION_POSITION_DELTA       = (1 << 3)
+        VISION_POSITION_DELTA       = (1 << 3),
+        ODOMETRY                    = (1 << 4),
     };
 
     // return true if the given message type should be sent