From fe173227de256cf0d1ed9b8778a54c9b77796235 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sat, 3 Mar 2012 11:10:39 +1100
Subject: [PATCH] DCM_Test: fixed example code to work with SITL

this also fixes the compass setup
---
 .../AP_DCM/examples/DCM_Test/DCM_Test.pde     | 58 ++++++++++++++-----
 libraries/AP_DCM/examples/DCM_Test/Makefile   |  3 +
 2 files changed, 46 insertions(+), 15 deletions(-)

diff --git a/libraries/AP_DCM/examples/DCM_Test/DCM_Test.pde b/libraries/AP_DCM/examples/DCM_Test/DCM_Test.pde
index 3f9c6da226..639bc27a2b 100644
--- a/libraries/AP_DCM/examples/DCM_Test/DCM_Test.pde
+++ b/libraries/AP_DCM/examples/DCM_Test/DCM_Test.pde
@@ -16,6 +16,11 @@
 #include <AP_Math.h>
 #include <AP_Common.h>
 #include <AP_Compass.h>
+#include <DataFlash.h>
+#include <APM_RC.h>
+#include <GCS_MAVLink.h>
+#include <AP_GPS.h>
+#include <AP_Baro.h>
 
 // uncomment this for a APM2 board
 // #define APM2_HARDWARE
@@ -25,7 +30,11 @@ FastSerialPort(Serial, 0);
 
 Arduino_Mega_ISR_Registry isr_registry;
 AP_TimerProcess  scheduler;
-AP_Compass_HMC5843 compass(AP_Var::k_key_none);
+#ifdef DESKTOP_BUILD
+AP_Compass_HIL     compass;
+#else
+AP_Compass_HMC5843 compass;
+#endif
 
 #ifdef APM2_HARDWARE
   AP_InertialSensor_MPU6000 ins( 53 );
@@ -36,7 +45,9 @@ AP_Compass_HMC5843 compass(AP_Var::k_key_none);
 
 static GPS         *g_gps;
 
-AP_IMU_INS imu( &ins, AP_Var::k_key_none );
+AP_Baro_BMP085_HIL      barometer;
+
+AP_IMU_INS imu( &ins);
 AP_DCM  dcm(&imu, g_gps);
 
 
@@ -69,9 +80,11 @@ void setup(void)
 	isr_registry.init();
 	scheduler.init(&isr_registry);
 
+#ifndef DESKTOP_BUILD
     I2c.begin();
     I2c.timeOut(5);
     I2c.setSpeed(true);
+#endif
 
     SPI.begin();
     SPI.setClockDivider(SPI_CLOCK_DIV16);
@@ -81,7 +94,7 @@ void setup(void)
 
 	compass.set_orientation(MAG_ORIENTATION);
 	if (compass.init()) {
-		printf("Enabling compass\n");
+		Serial.printf("Enabling compass\n");
 		compass.null_offsets_enable();
 		dcm.set_compass(&compass);
 	}
@@ -89,22 +102,37 @@ void setup(void)
 
 void loop(void)
 {
-	Vector3f	accel;
-	Vector3f	gyro;
-	static uint8_t counter;
+	static uint16_t counter;
+	static uint32_t last_t, last_print;
+	uint32_t now = micros();
+	float deltat;
 
-	delay(20);
+	if (last_t == 0) {
+		last_t = now;
+		return;
+	}
+	deltat = (now - last_t) * 1.0e-6;
+	last_t = now;
 
+	compass.read();
+	compass.calculate(dcm.get_dcm_matrix());
 	dcm.update_DCM();
-	if (counter++ == 10) {
-		counter = 0;
+	delay(20);
+	counter++;
+
+	if (now - last_print >= 0.5e6) {
+		Vector3f accel = imu.get_accel();
+		Vector3f gyro  = imu.get_gyro();
 		gyro  = imu.get_gyro();
 		accel = imu.get_accel();
-		Serial.printf_P(PSTR("r:%4d  p:%4d  y:%3d  g=(%5.1f %5.1f %5.1f)  a=(%5.1f %5.1f %5.1f)\n"),
-				(int)dcm.roll_sensor / 100,
-				(int)dcm.pitch_sensor / 100,
-				(uint16_t)dcm.yaw_sensor / 100,
-				gyro.x, gyro.y, gyro.z,
-				accel.x, accel.y, accel.z);
+		Serial.printf_P(PSTR("r:%4d  p:%4d  y:%3d  g=(%5.1f %5.1f %5.1f)  a=(%5.1f %5.1f %5.1f) rate=%.1f\n"),
+						(int)dcm.roll_sensor / 100,
+						(int)dcm.pitch_sensor / 100,
+						(uint16_t)dcm.yaw_sensor / 100,
+						gyro.x, gyro.y, gyro.z,
+						accel.x, accel.y, accel.z,
+						(1.0e6*counter)/(now-last_print));
+		last_print = now;
+		counter = 0;
 	}
 }
diff --git a/libraries/AP_DCM/examples/DCM_Test/Makefile b/libraries/AP_DCM/examples/DCM_Test/Makefile
index d1f40fd90f..fcdc8ff8fe 100644
--- a/libraries/AP_DCM/examples/DCM_Test/Makefile
+++ b/libraries/AP_DCM/examples/DCM_Test/Makefile
@@ -1 +1,4 @@
 include ../../../AP_Common/Arduino.mk
+
+sitl:
+	make -f ../../../../libraries/Desktop/Desktop.mk