From e58e8b861dbf4430a0123fecd1549954743f2aae Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 28 Nov 2023 14:18:14 +1100
Subject: [PATCH] AP_Serialmanager: added IMOUT uart type

# Conflicts:
#	libraries/AP_SerialManager/AP_SerialManager.cpp
#	libraries/AP_SerialManager/AP_SerialManager.h
---
 libraries/AP_SerialManager/AP_SerialManager.cpp    | 14 +++++++++++++-
 libraries/AP_SerialManager/AP_SerialManager.h      |  1 +
 .../AP_SerialManager/AP_SerialManager_config.h     | 10 ++++++++++
 3 files changed, 24 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_SerialManager/AP_SerialManager.cpp b/libraries/AP_SerialManager/AP_SerialManager.cpp
index 38b8124aaf..5b961da072 100644
--- a/libraries/AP_SerialManager/AP_SerialManager.cpp
+++ b/libraries/AP_SerialManager/AP_SerialManager.cpp
@@ -24,6 +24,7 @@
 #include <AP_Math/AP_Math.h>
 #include <AP_RCProtocol/AP_RCProtocol.h>
 #include <AP_MSP/AP_MSP.h>
+#include <AP_InertialSensor/AP_InertialSensor.h>
 #include "AP_SerialManager.h"
 #include <GCS_MAVLink/GCS.h>
 
@@ -209,7 +210,7 @@ const AP_Param::GroupInfo AP_SerialManager::var_info[] = {
     // @Param: 1_PROTOCOL
     // @DisplayName: Telem1 protocol selection
     // @Description: Control what protocol to use on the Telem1 port. Note that the Frsky options require external converter hardware. See the wiki for details.
-    // @Values: -1:None, 1:MAVLink1, 2:MAVLink2, 3:Frsky D, 4:Frsky SPort, 5:GPS, 7:Alexmos Gimbal Serial, 8:Gimbal, 9:Rangefinder, 10:FrSky SPort Passthrough (OpenTX), 11:Lidar360, 13:Beacon, 14:Volz servo out, 15:SBus servo out, 16:ESC Telemetry, 17:Devo Telemetry, 18:OpticalFlow, 19:RobotisServo, 20:NMEA Output, 21:WindVane, 22:SLCAN, 23:RCIN, 24:EFI Serial, 25:LTM, 26:RunCam, 27:HottTelem, 28:Scripting, 29:Crossfire VTX, 30:Generator, 31:Winch, 32:MSP, 33:DJI FPV, 34:AirSpeed, 35:ADSB, 36:AHRS, 37:SmartAudio, 38:FETtecOneWire, 39:Torqeedo, 40:AIS, 41:CoDevESC, 42:DisplayPort, 43:MAVLink High Latency, 44:IRC Tramp, 45:DDS XRCE
+    // @Values: -1:None, 1:MAVLink1, 2:MAVLink2, 3:Frsky D, 4:Frsky SPort, 5:GPS, 7:Alexmos Gimbal Serial, 8:Gimbal, 9:Rangefinder, 10:FrSky SPort Passthrough (OpenTX), 11:Lidar360, 13:Beacon, 14:Volz servo out, 15:SBus servo out, 16:ESC Telemetry, 17:Devo Telemetry, 18:OpticalFlow, 19:RobotisServo, 20:NMEA Output, 21:WindVane, 22:SLCAN, 23:RCIN, 24:EFI Serial, 25:LTM, 26:RunCam, 27:HottTelem, 28:Scripting, 29:Crossfire VTX, 30:Generator, 31:Winch, 32:MSP, 33:DJI FPV, 34:AirSpeed, 35:ADSB, 36:AHRS, 37:SmartAudio, 38:FETtecOneWire, 39:Torqeedo, 40:AIS, 41:CoDevESC, 42:DisplayPort, 43:MAVLink High Latency, 44:IRC Tramp, 45:DDS XRCE, 46:IMUDATA
     // @User: Standard
     // @RebootRequired: True
     AP_GROUPINFO("1_PROTOCOL",  1, AP_SerialManager, state[1].protocol, DEFAULT_SERIAL1_PROTOCOL),
@@ -600,6 +601,17 @@ void AP_SerialManager::init()
                     // Note init is handled by AP_MSP
                     break;
 #endif
+
+#if AP_SERIALMANAGER_IMUOUT_ENABLED
+                case SerialProtocol_IMUOUT:
+                    uart->begin(state[i].baudrate(),
+                                AP_SERIALMANAGER_IMUOUT_BUFSIZE_RX,
+                                AP_SERIALMANAGER_IMUOUT_BUFSIZE_TX);
+                    AP::ins().set_imu_out_uart(uart);
+                    uart->set_unbuffered_writes(true);
+                    break;
+#endif
+
                 default:
                     uart->begin(state[i].baudrate());
             }
diff --git a/libraries/AP_SerialManager/AP_SerialManager.h b/libraries/AP_SerialManager/AP_SerialManager.h
index 01d5d7bbd4..b548656297 100644
--- a/libraries/AP_SerialManager/AP_SerialManager.h
+++ b/libraries/AP_SerialManager/AP_SerialManager.h
@@ -79,6 +79,7 @@ public:
         SerialProtocol_MAVLinkHL = 43,
         SerialProtocol_Tramp = 44,
         SerialProtocol_DDS_XRCE = 45,
+        SerialProtocol_IMUOUT = 46,
         SerialProtocol_NumProtocols                    // must be the last value
     };
 
diff --git a/libraries/AP_SerialManager/AP_SerialManager_config.h b/libraries/AP_SerialManager/AP_SerialManager_config.h
index 7dbf0c16a8..801b52a163 100644
--- a/libraries/AP_SerialManager/AP_SerialManager_config.h
+++ b/libraries/AP_SerialManager/AP_SerialManager_config.h
@@ -21,6 +21,7 @@
 
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Networking/AP_Networking_Config.h>
+#include <AP_InertialSensor/AP_InertialSensor_config.h>
 
 #ifdef HAL_UART_NUM_SERIAL_PORTS
 #if HAL_UART_NUM_SERIAL_PORTS >= 4
@@ -56,6 +57,10 @@
 #define AP_SERIALMANAGER_REGISTER_ENABLED BOARD_FLASH_SIZE > 1024 && (AP_NETWORKING_ENABLED || HAL_ENABLE_DRONECAN_DRIVERS)
 #endif
 
+#ifndef AP_SERIALMANAGER_IMUOUT_ENABLED
+#define AP_SERIALMANAGER_IMUOUT_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) && AP_INERTIALSENSOR_ENABLED
+#endif
+
 // serial ports registered by AP_Networking will use IDs starting at 21 for the first port
 #define AP_SERIALMANAGER_NET_PORT_1         21 // NET_P1_*
 
@@ -127,3 +132,8 @@
 #define AP_SERIALMANAGER_MSP_BUFSIZE_RX     128
 #define AP_SERIALMANAGER_MSP_BUFSIZE_TX     256
 #define AP_SERIALMANAGER_MSP_BAUD           115200
+
+// IMU OUT protocol
+#define AP_SERIALMANAGER_IMUOUT_BAUD           921600
+#define AP_SERIALMANAGER_IMUOUT_BUFSIZE_RX     128
+#define AP_SERIALMANAGER_IMUOUT_BUFSIZE_TX     2048