From e9e46f9c9dc1301b4218903b26dbcd58fb096895 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 12 Sep 2013 15:49:04 +1000
Subject: [PATCH] px4io: added monitoring of vservo and vrssi

publish via servorail_status ORB topic
---
 src/drivers/px4io/px4io.cpp           | 41 +++++++++++++++++++++++++--
 src/modules/px4iofirmware/registers.c | 30 +++++++++-----------
 2 files changed, 52 insertions(+), 19 deletions(-)

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index af20e61cbd..27b6a12da4 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -80,6 +80,7 @@
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/rc_channels.h>
 #include <uORB/topics/battery_status.h>
+#include <uORB/topics/servorail_status.h>
 #include <uORB/topics/parameter_update.h>
 #include <debug.h>
 
@@ -260,6 +261,7 @@ private:
 	orb_advert_t 		_to_actuators_effective; ///< effective actuator controls topic
 	orb_advert_t		_to_outputs;		///< mixed servo outputs topic
 	orb_advert_t		_to_battery;		///< battery status / voltage
+	orb_advert_t		_to_servorail;		///< servorail status
 	orb_advert_t		_to_safety;		///< status of safety
 
 	actuator_outputs_s	_outputs;		///<mixed outputs
@@ -414,10 +416,20 @@ private:
 	 *
 	 * Publish IO battery information if necessary.
 	 *
-	 * @param vbatt		vbattery register
-	 * @param status	ibatter register
+	 * @param vbatt		vbatt register
+	 * @param ibatt		ibatt register
 	 */
 	void			io_handle_battery(uint16_t vbatt, uint16_t ibatt);
+
+	/**
+	 * Handle a servorail update from IO.
+	 *
+	 * Publish servo rail information if necessary.
+	 *
+	 * @param vservo	vservo register
+	 * @param vrssi 	vrssi register
+	 */
+	void			io_handle_vservo(uint16_t vbatt, uint16_t ibatt);
         
 };
 
@@ -453,6 +465,7 @@ PX4IO::PX4IO(device::Device *interface) :
 	_to_actuators_effective(0),
 	_to_outputs(0),
 	_to_battery(0),
+	_to_servorail(0),
 	_to_safety(0),
 	_primary_pwm_device(false),
 	_battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor
@@ -1206,10 +1219,28 @@ PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt)
 	}
 }
 
+void
+PX4IO::io_handle_vservo(uint16_t vservo, uint16_t vrssi)
+{
+	servorail_status_s servorail_status;
+	servorail_status.timestamp = hrt_absolute_time();
+
+	/* voltage is scaled to mV */
+	servorail_status.voltage_v = vservo * 0.001f;
+	servorail_status.rssi_v    = vrssi * 0.001f;
+	
+	/* lazily publish the servorail voltages */
+	if (_to_servorail > 0) {
+		orb_publish(ORB_ID(servorail_status), _to_servorail, &servorail_status);
+	} else {
+		_to_servorail = orb_advertise(ORB_ID(servorail_status), &servorail_status);
+	}
+}
+
 int
 PX4IO::io_get_status()
 {
-	uint16_t	regs[4];
+	uint16_t	regs[6];
 	int		ret;
 
 	/* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */
@@ -1224,6 +1255,10 @@ PX4IO::io_get_status()
 	io_handle_battery(regs[2], regs[3]);
 #endif
 
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+	io_handle_vservo(regs[4], regs[5]);
+#endif
+
 	return ret;
 }
 
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 9c95fd1c52..30ef0cceaf 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -678,27 +678,25 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
 #ifdef ADC_VSERVO
 		/* PX4IO_P_STATUS_VSERVO */
 		{
-			/*
-			 * Coefficients here derived by measurement of the 5-16V
-			 * range on one unit:
-			 *
-			 * XXX pending measurements
-			 *
-			 * slope = xxx
-			 * intercept = xxx
-			 *
-			 * Intercept corrected for best results @ 5.0V.
-			 */
 			unsigned counts = adc_measure(ADC_VSERVO);
 			if (counts != 0xffff) {
-				unsigned mV = (4150 + (counts * 46)) / 10 - 200;
-				unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VSERVO_SCALE]) / 10000;
-
-				r_page_status[PX4IO_P_STATUS_VSERVO] = corrected;
+				// use 3:1 scaling on 3.3V ADC input
+				unsigned mV = counts * 9900 / 4096;
+				r_page_status[PX4IO_P_STATUS_VSERVO] = mV;
+			}
+		}
+#endif
+#ifdef ADC_RSSI
+		/* PX4IO_P_STATUS_VRSSI */
+		{
+			unsigned counts = adc_measure(ADC_RSSI);
+			if (counts != 0xffff) {
+				// use 1:1 scaling on 3.3V ADC input
+				unsigned mV = counts * 3300 / 4096;
+				r_page_status[PX4IO_P_STATUS_VRSSI] = mV;
 			}
 		}
 #endif
-		/* XXX PX4IO_P_STATUS_VRSSI */
 		/* XXX PX4IO_P_STATUS_PRSSI */
 
 		SELECT_PAGE(r_page_status);