ACM: added SIMSTATE, DCM and HWSTATUS messages to ACM

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);
 		}
 	}
 }

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
 };