From a6e87915eb0410fba01a49ac88fc05aa752f2b5f Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Wed, 11 Jan 2012 21:30:48 +1100
Subject: [PATCH] desktop: floating point precision changes from Justin Beech

---
 libraries/Desktop/support/desktop.h    |  8 +++----
 libraries/Desktop/support/sitl_adc.cpp | 30 ++++++++++++++++++++------
 libraries/Desktop/support/sitl_gps.cpp |  6 +++---
 3 files changed, 31 insertions(+), 13 deletions(-)

diff --git a/libraries/Desktop/support/desktop.h b/libraries/Desktop/support/desktop.h
index e373130e78..d3ac289e3d 100644
--- a/libraries/Desktop/support/desktop.h
+++ b/libraries/Desktop/support/desktop.h
@@ -20,11 +20,11 @@ void sitl_setup(void);
 int sitl_gps_pipe(void);
 ssize_t sitl_gps_read(int fd, void *buf, size_t count);
 void sitl_update_compass(float heading, float roll, float pitch, float yaw);
-void sitl_update_gps(float latitude, float longitude, float altitude,
-		     float speedN, float speedE, bool have_lock);
+void sitl_update_gps(double latitude, double longitude, float altitude,
+		     double speedN, double speedE, bool have_lock);
 void sitl_update_adc(float roll, float pitch, float yaw,
-		     float rollRate, float pitchRate, float yawRate,
-		     float xAccel, float yAccel, float zAccel,
+		     double rollRate, double pitchRate, double yawRate,
+		     double xAccel, double yAccel, double zAccel,
 		     float airspeed);
 void sitl_setup_adc(void);
 void sitl_update_barometer(float altitude);
diff --git a/libraries/Desktop/support/sitl_adc.cpp b/libraries/Desktop/support/sitl_adc.cpp
index 8ea7de885d..63e44c2111 100644
--- a/libraries/Desktop/support/sitl_adc.cpp
+++ b/libraries/Desktop/support/sitl_adc.cpp
@@ -40,10 +40,28 @@ static uint16_t airspeed_sensor(float airspeed)
   average rates to cope with slow update rates.
 
   inputs are in degrees
+
+	phi - roll
+	theta - pitch
+	psi - true heading
+	alpha - angle of attack
+	beta - side slip
+	phidot - roll rate
+	thetadot - pitch rate
+	psidot - yaw rate
+	v_north - north velocity in local/body frame
+	v_east - east velocity in local/body frame
+	v_down - down velocity in local/body frame
+	A_X_pilot - X accel in body frame
+	A_Y_pilot - Y accel in body frame
+	A_Z_pilot - Z accel in body frame
+
+  Note: doubles on high prec. stuff are preserved until the last moment
+
  */
-void sitl_update_adc(float roll, float pitch, float yaw,
-		     float rollRate, float pitchRate, float yawRate,
-		     float xAccel, float yAccel, float zAccel,
+void sitl_update_adc(float roll, 	float pitch, 	float yaw,		// Relative to earth
+		     double rollRate, 	double pitchRate,double yawRate,	// Local to plane
+		     double xAccel, 	double yAccel, 	double zAccel,		// Local to plane
 		     float airspeed)
 {
 	static const uint8_t sensor_map[6] = { 1, 2, 0, 4, 5, 6 };
@@ -55,9 +73,9 @@ void sitl_update_adc(float roll, float pitch, float yaw,
 	const float _gyro_gain_y = ToRad(0.41);
 	const float _gyro_gain_z = ToRad(0.41);
 	const float _accel_scale = 9.80665 / 423.8;
-	float adc[7];
-	float phi, theta, phiDot, thetaDot, psiDot;
-	float p, q, r;
+	double adc[7];
+	double phi, theta, phiDot, thetaDot, psiDot;
+	double p, q, r;
 
 	/* convert the angular velocities from earth frame to
 	   body frame. Thanks to James Goppert for the formula
diff --git a/libraries/Desktop/support/sitl_gps.cpp b/libraries/Desktop/support/sitl_gps.cpp
index b21dc1b1a9..11a26711f8 100644
--- a/libraries/Desktop/support/sitl_gps.cpp
+++ b/libraries/Desktop/support/sitl_gps.cpp
@@ -78,8 +78,8 @@ static void gps_send(uint8_t msgid, uint8_t *buf, uint16_t size)
 /*
   possibly send a new GPS UBLOX packet
  */
-void sitl_update_gps(float latitude, float longitude, float altitude,
-		     float speedN, float speedE, bool have_lock)
+void sitl_update_gps(double latitude, double longitude, float altitude,
+		     double speedN, double speedE, bool have_lock)
 {
 	struct ubx_nav_posllh {
 		uint32_t	time; // GPS msToW
@@ -113,7 +113,7 @@ void sitl_update_gps(float latitude, float longitude, float altitude,
         const uint8_t MSG_POSLLH = 0x2;
 	const uint8_t MSG_STATUS = 0x3;
 	const uint8_t MSG_VELNED = 0x12;
-	float lon_scale;
+	double lon_scale;
 
 	// 4Hz
 	if (millis() - gps_state.last_update < 250) {