From b7d53a941e8bfc738656138f6f840d88db72e476 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 1 Apr 2020 21:11:32 +1100
Subject: [PATCH] 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
---
 .../AP_InertialSensor/AP_InertialSensor.cpp   | 51 +++++++++++++++++++
 .../AP_InertialSensor/AP_InertialSensor.h     | 11 ++++
 2 files changed, 62 insertions(+)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
index 18dd8967dfbe2b..f2694481109cc3 100644
--- 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
index db25f3d988f5cf..a8acfc0f68a704 100644
--- 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,