From 7e64cfcc93dd61505dec959f0dc3acb862c63cbd Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sat, 19 Nov 2011 10:07:11 +1100
Subject: [PATCH] desktop: normalise roll and pitch rates

---
 libraries/Desktop/support/sitl.cpp     |  4 ++--
 libraries/Desktop/support/sitl_adc.cpp | 19 +++++++++++--------
 libraries/Desktop/support/util.cpp     | 11 +++++++++++
 libraries/Desktop/support/util.h       |  1 +
 4 files changed, 25 insertions(+), 10 deletions(-)

diff --git a/libraries/Desktop/support/sitl.cpp b/libraries/Desktop/support/sitl.cpp
index ec840bceb0..65622fe4bb 100644
--- a/libraries/Desktop/support/sitl.cpp
+++ b/libraries/Desktop/support/sitl.cpp
@@ -126,9 +126,9 @@ static void sitl_fgear_input(void)
 		sitl_update_gps(d.fg_pkt.latitude, d.fg_pkt.longitude,
 				ft2m(d.fg_pkt.altitude),
 				ft2m(d.fg_pkt.speedN), ft2m(d.fg_pkt.speedE));
-		sitl_update_adc(d.fg_pkt.rollDeg, d.fg_pkt.pitchDeg, d.fg_pkt.yawDeg, kt2mps(d.fg_pkt.airspeed));
+		sitl_update_adc(d.fg_pkt.rollDeg, d.fg_pkt.pitchDeg, d.fg_pkt.heading, kt2mps(d.fg_pkt.airspeed));
 		sitl_update_barometer(ft2m(d.fg_pkt.altitude));
-		sitl_update_compass(d.fg_pkt.heading, d.fg_pkt.rollDeg, d.fg_pkt.pitchDeg, d.fg_pkt.yawDeg);
+		sitl_update_compass(d.fg_pkt.heading, d.fg_pkt.rollDeg, d.fg_pkt.pitchDeg, d.fg_pkt.heading);
 		count++;
 		sim_input_count++;
 		if (millis() - last_report > 1000) {
diff --git a/libraries/Desktop/support/sitl_adc.cpp b/libraries/Desktop/support/sitl_adc.cpp
index 5d2405fc5a..e127446aa3 100644
--- a/libraries/Desktop/support/sitl_adc.cpp
+++ b/libraries/Desktop/support/sitl_adc.cpp
@@ -57,7 +57,7 @@ void sitl_update_adc(float roll, float pitch, float yaw, float airspeed)
 	float rollRate, pitchRate, yawRate;
 	static uint32_t last_update;
 	static float last_roll, last_pitch, last_yaw;
-	uint32_t delta_t;
+	unsigned long delta_t;
 
 	/* map roll/pitch/yaw to values the accelerometer would see */
 	xAccel = sin(ToRad(pitch));
@@ -73,12 +73,15 @@ void sitl_update_adc(float roll, float pitch, float yaw, float airspeed)
 		rollRate = 0;
 		pitchRate = 0;
 		yawRate = 0;
-		delta_t = millis();
+		delta_t = micros();
 	} else {
-		delta_t = millis() - last_update;
-		rollRate  = 1000.0 * (roll  - last_roll)  / delta_t;
-		pitchRate = 1000.0 * (pitch - last_pitch) / delta_t;
-		yawRate   = 1000.0 * (yaw   - last_yaw)   / delta_t;
+		delta_t = micros() - last_update;
+		rollRate  = 1.0e6 * (roll  - last_roll)  / delta_t;
+		pitchRate = 1.0e6 * (pitch - last_pitch) / delta_t;
+		yawRate   = 1.0e6 * (yaw   - last_yaw)   / delta_t;
+		rollRate = normalise(rollRate, -180, 180);
+		pitchRate = normalise(pitchRate, -180, 180);
+		yawRate = normalise(yawRate, -180, 180);
 	}
 	last_update += delta_t;
 
@@ -109,8 +112,8 @@ void sitl_update_adc(float roll, float pitch, float yaw, float airspeed)
 
 #if 0
 	extern AP_DCM dcm;
-	printf("roll=%6.1f pitch=%6.1f dcm_roll=%6.1f dcm_pitch=%6.1f rollRate=%6.3f pitchRate=%6.3f\n",
-	       roll, pitch, dcm.roll_sensor/100.0, dcm.pitch_sensor/100.0, rollRate, pitchRate);
+	printf("roll=%6.1f / %6.1f pitch=%6.1f / %6.1f  rollRate=%6.1f pitchRate=%6.1f\n",
+	       roll, dcm.roll_sensor/100.0, pitch, dcm.pitch_sensor/100.0, rollRate, pitchRate);
 #endif
 }
 
diff --git a/libraries/Desktop/support/util.cpp b/libraries/Desktop/support/util.cpp
index e053bde264..702864851d 100644
--- a/libraries/Desktop/support/util.cpp
+++ b/libraries/Desktop/support/util.cpp
@@ -79,3 +79,14 @@ void set_nonblocking(int fd)
 	unsigned v = fcntl(fd, F_GETFL, 0);
 	fcntl(fd, F_SETFL, v | O_NONBLOCK);
 }
+
+double normalise(double v, double min, double max)
+{
+	while (v < min) {
+		v += (max - min);
+	}
+	while (v > max) {
+		v -= (max - min);
+	}
+	return v;
+}
diff --git a/libraries/Desktop/support/util.h b/libraries/Desktop/support/util.h
index a6ff07ae05..5f075184f9 100644
--- a/libraries/Desktop/support/util.h
+++ b/libraries/Desktop/support/util.h
@@ -8,3 +8,4 @@ void swap_doubles(double *d, unsigned count);
 float swap_float(float f);
 void swap_floats(float *f, unsigned count);
 void set_nonblocking(int fd);
+double normalise(double v, double min, double max);