From 36cca7a31b428408eb686df592f092ba5fc24006 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 4 Aug 2013 19:10:56 +0200
Subject: [PATCH] Added rate limit in sensors app. Just pending accel filters
 now

---
 src/modules/sensors/sensors.cpp | 66 +++++++++++++++++++++++++++++++--
 1 file changed, 63 insertions(+), 3 deletions(-)

diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 7ff9e19b4c..0f1782fca6 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -92,8 +92,35 @@
 #define BARO_HEALTH_COUNTER_LIMIT_OK 5
 #define ADC_HEALTH_COUNTER_LIMIT_OK  5
 
-#define ADC_BATTERY_VOLTAGE_CHANNEL	10
-#define ADC_AIRSPEED_VOLTAGE_CHANNEL	11
+/**
+ * Analog layout:
+ * FMU:
+ * IN2 - battery voltage
+ * IN3 - battery current
+ * IN4 - 5V sense
+ * IN10 - spare (we could actually trim these from the set)
+ * IN11 - spare (we could actually trim these from the set)
+ * IN12 - spare (we could actually trim these from the set)
+ * IN13 - aux1
+ * IN14 - aux2
+ * IN15 - pressure sensor
+ * 
+ * IO:
+ * IN4 - servo supply rail
+ * IN5 - analog RSSI
+ */
+
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+  #define ADC_BATTERY_VOLTAGE_CHANNEL	10
+  #define ADC_AIRSPEED_VOLTAGE_CHANNEL	11
+#endif
+
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+  #define ADC_BATTERY_VOLTAGE_CHANNEL	2
+  #define ADC_BATTERY_CURRENT_CHANNEL	3
+  #define ADC_5V_RAIL_SENSE		4
+  #define ADC_AIRSPEED_VOLTAGE_CHANNEL	15
+#endif
 
 #define BAT_VOL_INITIAL 0.f
 #define BAT_VOL_LOWPASS_1 0.99f
@@ -731,12 +758,27 @@ Sensors::accel_init()
 		errx(1, "FATAL: no accelerometer found");
 
 	} else {
+
+		// XXX do the check more elegantly
+
+		#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+
 		/* set the accel internal sampling rate up to at leat 500Hz */
 		ioctl(fd, ACCELIOCSSAMPLERATE, 500);
 
 		/* set the driver to poll at 500Hz */
 		ioctl(fd, SENSORIOCSPOLLRATE, 500);
 
+		#else
+
+		/* set the accel internal sampling rate up to at leat 800Hz */
+		ioctl(fd, ACCELIOCSSAMPLERATE, 800);
+
+		/* set the driver to poll at 800Hz */
+		ioctl(fd, SENSORIOCSPOLLRATE, 800);
+
+		#endif
+
 		warnx("using system accel");
 		close(fd);
 	}
@@ -754,12 +796,27 @@ Sensors::gyro_init()
 		errx(1, "FATAL: no gyro found");
 
 	} else {
+
+		// XXX do the check more elegantly
+
+		#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+
 		/* set the gyro internal sampling rate up to at leat 500Hz */
 		ioctl(fd, GYROIOCSSAMPLERATE, 500);
 
 		/* set the driver to poll at 500Hz */
 		ioctl(fd, SENSORIOCSPOLLRATE, 500);
 
+		#else
+
+		/* set the gyro internal sampling rate up to at leat 800Hz */
+		ioctl(fd, GYROIOCSSAMPLERATE, 800);
+
+		/* set the driver to poll at 800Hz */
+		ioctl(fd, SENSORIOCSPOLLRATE, 800);
+
+		#endif
+
 		warnx("using system gyro");
 		close(fd);
 	}
@@ -1360,6 +1417,9 @@ Sensors::task_main()
 	/* rate limit vehicle status updates to 5Hz */
 	orb_set_interval(_vstatus_sub, 200);
 
+	/* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */
+	orb_set_interval(_gyro_sub, 4);
+
 	/*
 	 * do advertisements
 	 */
@@ -1395,7 +1455,7 @@ Sensors::task_main()
 
 	while (!_task_should_exit) {
 
-		/* wait for up to 500ms for data */
+		/* wait for up to 100ms for data */
 		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
 
 		/* timed out - periodic check for _task_should_exit, etc. */