From b07a3eb96678a4acd5b11b0713d3736c3b24237b Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 1 Mar 2012 23:36:35 +1100
Subject: [PATCH] ACM: added SIMSTATE, DCM and HWSTATUS messages to ACM

---
 ArduCopter/GCS_Mavlink.pde | 62 ++++++++++++++++++++++++++++++++++++--
 ArduCopter/defines.h       |  3 ++
 2 files changed, 63 insertions(+), 2 deletions(-)

diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde
index 4a45b181b2..9ac2fd7e07 100644
--- a/ArduCopter/GCS_Mavlink.pde
+++ b/ArduCopter/GCS_Mavlink.pde
@@ -124,6 +124,42 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
         crosstrack_error);	// was 0
 }
 
+#if HIL_MODE != HIL_MODE_ATTITUDE
+static void NOINLINE send_dcm(mavlink_channel_t chan)
+{
+    Vector3f omega_I = dcm.get_integrator();
+    mavlink_msg_dcm_send(
+        chan,
+        omega_I.x,
+        omega_I.y,
+        omega_I.z,
+        dcm.get_accel_weight(),
+        dcm.get_renorm_val(),
+        dcm.get_error_rp(),
+        dcm.get_error_yaw());
+}
+#endif // HIL_MODE != HIL_MODE_ATTITUDE
+
+#ifdef DESKTOP_BUILD
+// report simulator state
+static void NOINLINE send_simstate(mavlink_channel_t chan)
+{
+    extern void sitl_simstate_send(uint8_t chan);
+    sitl_simstate_send((uint8_t)chan);
+}
+#endif
+
+#ifndef DESKTOP_BUILD
+static void NOINLINE send_hwstatus(mavlink_channel_t chan)
+{
+    mavlink_msg_hwstatus_send(
+        chan,
+        board_voltage(),
+        I2c.lockup_count());
+}
+#endif
+
+
 static void NOINLINE send_gps_raw(mavlink_channel_t chan)
 {
     mavlink_msg_gps_raw_send(
@@ -435,6 +471,27 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
         send_statustext(chan);
         break;
 
+    case MSG_DCM:
+#if HIL_MODE != HIL_MODE_ATTITUDE
+        CHECK_PAYLOAD_SIZE(DCM);
+        send_dcm(chan);
+#endif
+        break;
+
+    case MSG_SIMSTATE:
+#ifdef DESKTOP_BUILD
+        CHECK_PAYLOAD_SIZE(DCM);
+        send_simstate(chan);
+#endif
+        break;
+
+    case MSG_HWSTATUS:
+#ifndef DESKTOP_BUILD
+        CHECK_PAYLOAD_SIZE(HWSTATUS);
+        send_hwstatus(chan);
+#endif
+        break;
+
     case MSG_RETRY_DEFERRED:
         break; // just here to prevent a warning
 	}
@@ -669,6 +726,7 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
 
 		if (freqLoopMatch(streamRateExtra1, freqMin, freqMax)){	 // Use Extra 1 for AHRS info
 			send_message(MSG_ATTITUDE);
+			send_message(MSG_SIMSTATE);
 			//Serial.printf("mav6 %d\n", (int)streamRateExtra1.get());
 		}
 
@@ -678,8 +736,8 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
 		}
 
 		if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){
-		// Available datastream
-			//Serial.printf("mav8 %d\n", (int)streamRateExtra3.get());
+			send_message(MSG_DCM);
+			send_message(MSG_HWSTATUS);
 		}
 	}
 }
diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h
index 3a22e84eb6..906f960d52 100644
--- a/ArduCopter/defines.h
+++ b/ArduCopter/defines.h
@@ -234,6 +234,9 @@ enum ap_message {
     MSG_NEXT_WAYPOINT,
     MSG_NEXT_PARAM,
     MSG_STATUSTEXT,
+    MSG_DCM,
+    MSG_SIMSTATE,
+    MSG_HWSTATUS,
     MSG_RETRY_DEFERRED // this must be last
 };