AP_InertialSensor: added support for writing raw IMU data to a UART

this is for supporting external visual odomotry systems which need the IMU data to correctly process image data
ArduPilot · Apr 1, 2020 · b7d53a9 · b7d53a9
1 parent a3e4058
commit b7d53a9
Show file tree

Hide file tree

Showing 2 changed files with 62 additions and 0 deletions.
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@@ -1391,6 +1391,12 @@ void AP_InertialSensor::update(void)
     _last_update_usec = AP_HAL::micros();
 
     _have_sample = false;
+
+#if !HAL_MINIMIZE_FEATURES
+    if (uart.imu_out_uart) {
+        send_uart_data();
+    }
+#endif
 }
 
 /*
@@ -2129,6 +2135,51 @@ void AP_InertialSensor::kill_imu(uint8_t imu_idx, bool kill_it)
 }
 #endif // HAL_MINIMIZE_FEATURES
 
+/*
+  setup a UART for sending external data
+ */
+void AP_InertialSensor::set_imu_out_uart(AP_HAL::UARTDriver *_uart)
+{
+#if !HAL_MINIMIZE_FEATURES
+    uart.imu_out_uart = _uart;
+    uart.counter = 0;
+#endif
+}
+
+/*
+  send IMU delta-angle and delta-velocity to a UART
+ */
+void AP_InertialSensor::send_uart_data(void)
+{
+#if !HAL_MINIMIZE_FEATURES
+    struct {
+        uint16_t magic = 0x29c4;
+        uint16_t length;
+        uint32_t timestamp_us;
+        Vector3f delta_velocity;
+        Vector3f delta_angle;
+        float    delta_velocity_dt;
+        float    delta_angle_dt;
+        uint16_t counter;
+        uint16_t crc;
+    } data;
+
+    data.length = sizeof(data);
+    data.timestamp_us = AP_HAL::micros();
+
+    get_delta_angle(data.delta_angle);
+    data.delta_angle_dt = get_delta_angle_dt();
+
+    get_delta_velocity(data.delta_velocity);
+    data.delta_velocity_dt = get_delta_velocity_dt();
+
+    data.counter = uart.counter++;
+    data.crc = crc_xmodem((const uint8_t *)&data, sizeof(data)-sizeof(uint16_t));
+
+    uart.imu_out_uart->write((const uint8_t *)&data, sizeof(data));
+#endif
+}
+
 
 namespace AP {
 

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
@@ -315,6 +315,17 @@ class AP_InertialSensor : AP_AccelCal_Client
     // for killing an IMU for testing purposes
     void kill_imu(uint8_t imu_idx, bool kill_it);
 
+    // optional UART for sending IMU data to an external process
+    void set_imu_out_uart(AP_HAL::UARTDriver *uart);
+    void send_uart_data(void);
+
+#if !HAL_MINIMIZE_FEATURES
+    struct {
+        uint16_t counter;
+        AP_HAL::UARTDriver *imu_out_uart;
+    } uart;
+#endif
+
     enum IMU_SENSOR_TYPE {
         IMU_SENSOR_TYPE_ACCEL = 0,
         IMU_SENSOR_TYPE_GYRO = 1,