From c1f1208c893b2eda5bec1f7cfb95c02733623df1 Mon Sep 17 00:00:00 2001
From: Pierre Kancir <pierre.kancir.emn@gmail.com>
Date: Fri, 15 Jan 2021 08:54:15 +0100
Subject: [PATCH] SITL: add support for SIM_STATE msg

---
 libraries/SITL/SITL.cpp | 35 ++++++++++++++++++++++++++++++++++-
 libraries/SITL/SITL.h   |  1 +
 2 files changed, 35 insertions(+), 1 deletion(-)

diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp
index 848d528222..4dcb7d2b99 100644
--- a/libraries/SITL/SITL.cpp
+++ b/libraries/SITL/SITL.cpp
@@ -371,7 +371,7 @@ const AP_Param::GroupInfo SITL::var_sfml_joystick[] = {
 
 #endif //SFML_JOYSTICK
     
-/* report SITL state via MAVLink */
+/* report SITL state via MAVLink SIMSTATE*/
 void SITL::simstate_send(mavlink_channel_t chan)
 {
     float yaw;
@@ -396,6 +396,39 @@ void SITL::simstate_send(mavlink_channel_t chan)
                               state.longitude*1.0e7);
 }
 
+/* report SITL state via MAVLink SIM_STATE */
+void SITL::sim_state_send(mavlink_channel_t chan)
+{
+    // convert to same conventions as DCM
+    float yaw = state.yawDeg;
+    if (yaw > 180) {
+        yaw -= 360;
+    }
+
+    mavlink_msg_sim_state_send(chan,
+            state.quaternion.q1,
+            state.quaternion.q2,
+            state.quaternion.q3,
+            state.quaternion.q4,
+            ToRad(state.rollDeg),
+            ToRad(state.pitchDeg),
+            ToRad(yaw),
+            state.xAccel,
+            state.yAccel,
+            state.zAccel,
+            radians(state.rollRate),
+            radians(state.pitchRate),
+            radians(state.yawRate),
+            state.latitude*1.0e7,
+            state.longitude*1.0e7,
+            (float)state.altitude,
+            0.0,
+            0.0,
+            state.speedN,
+            state.speedE,
+            state.speedD);
+}
+
 /* report SITL state to AP_Logger */
 void SITL::Log_Write_SIMSTATE()
 {
diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h
index b384ae6884..f63040c1a3 100644
--- a/libraries/SITL/SITL.h
+++ b/libraries/SITL/SITL.h
@@ -387,6 +387,7 @@ public:
     time_t start_time_UTC;
 
     void simstate_send(mavlink_channel_t chan);
+    void sim_state_send(mavlink_channel_t chan);
 
     void Log_Write_SIMSTATE();