Add ADC measurements and reporting to PX4IO, including calibration for the battery input.

1 files changed, 54 insertions, 12 deletions

diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c
index 83a006d43..eaaea7817 100644
--- a/apps/px4io/comms.c
+++ b/apps/px4io/comms.c
@@ -61,8 +61,7 @@
 #define FMU_MIN_REPORT_INTERVAL	  5000	/*  5ms */
 #define FMU_MAX_REPORT_INTERVAL	100000	/* 100ms */
 
-int frame_rx;
-int frame_bad;
+#define FMU_STATUS_INTERVAL	1000000	/* 100ms */
 
 static int			fmu_fd;
 static hx_stream_t		stream;
@@ -87,6 +86,9 @@ comms_init(void)
 	cfsetspeed(&t, 115200);
 	t.c_cflag &= ~(CSTOPB | PARENB);
 	tcsetattr(fmu_fd, TCSANOW, &t);
+
+	/* init the ADC */
+	adc_init();
 }
 
 void
@@ -135,9 +137,55 @@ comms_main(void)
 			report.channel_count = system_state.rc_channels;
 			report.armed = system_state.armed;
 
+			report.battery_mv = system_state.battery_mv;
+			report.adc_in = system_state.adc_in5;
+			report.overcurrent = system_state.overcurrent;
+
 			/* and send it */
 			hx_stream_send(stream, &report, sizeof(report));
 		}
+
+		/*
+		 * Fetch ADC values, check overcurrent flags, etc.
+		 */
+		static hrt_abstime last_status_time;
+
+		if ((now - last_status_time) > FMU_STATUS_INTERVAL) {
+
+			/*
+			 * Coefficients here derived by measurement of the 5-16V
+			 * range on one unit:
+			 *
+			 * V   counts
+			 *  5  1001
+			 *  6  1219
+			 *  7  1436
+			 *  8  1653
+			 *  9  1870
+			 * 10  2086
+			 * 11  2303
+			 * 12  2522
+			 * 13  2738
+			 * 14  2956
+			 * 15  3172
+			 * 16  3389
+			 *
+			 * slope = 0.0046067
+			 * intercept = 0.3863
+			 *
+			 * Intercept corrected for best results @ 12V.
+			 */
+			unsigned counts = adc_measure(ADC_VBATT);
+			system_state.battery_mv = (4150 + (counts * 46)) / 10;
+
+			system_state.adc_in5 = adc_measure(ADC_IN5);
+
+			system_state.overcurrent = 
+				(OVERCURRENT_SERVO ? (1 << 0) : 0) |
+				(OVERCURRENT_ACC   ? (1 << 1) : 0);
+
+			last_status_time = now;
+		}
 	}
 }
 
@@ -146,12 +194,10 @@ comms_handle_config(const void *buffer, size_t length)
 {
 	const struct px4io_config *cfg = (struct px4io_config *)buffer;
 
-	if (length != sizeof(*cfg)) {
-		frame_bad++;
+	if (length != sizeof(*cfg))
 		return;
-	}
 
-	frame_rx++;
+	/* XXX */
 }
 
 static void
@@ -159,12 +205,9 @@ comms_handle_command(const void *buffer, size_t length)
 {
 	const struct px4io_command *cmd = (struct px4io_command *)buffer;
 
-	if (length != sizeof(*cmd)) {
-		frame_bad++;
+	if (length != sizeof(*cmd))
 		return;
-	}
-
-	frame_rx++;
+	
 	irqstate_t flags = irqsave();
 
 	/* fetch new PWM output values */
@@ -209,7 +252,6 @@ comms_handle_frame(void *arg, const void *buffer, size_t length)
 			comms_handle_config(buffer, length);
 			break;
 		default:
-		    	frame_bad++;
 			break;
 		}
 	}