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

3 files changed, 78 insertions, 20 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;
 		}
 	}
diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h
index c704b1201..a64e2f27d 100644
--- a/apps/px4io/protocol.h
+++ b/apps/px4io/protocol.h
@@ -73,6 +73,10 @@ struct px4io_report {
 	uint16_t	rc_channel[PX4IO_INPUT_CHANNELS];
 	bool		armed;
 	uint8_t		channel_count;
+
+	uint16_t	battery_mv;
+	uint16_t	adc_in;
+	uint8_t		overcurrent;
 };
 
 #pragma pack(pop)
\ No newline at end of file
diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h
index 6221b08f2..af06c6ec1 100644
--- a/apps/px4io/px4io.h
+++ b/apps/px4io/px4io.h
@@ -105,17 +105,25 @@ struct sys_state_s
 	bool		fmu_data_received;
 
 	/*
-	 * Current serial interface mode, per the serial_rx_mode parameter
-	 * in the config packet.
+	 * Measured battery voltage in mV
 	 */
-	uint8_t		serial_rx_mode;
+	uint16_t	battery_mv;
+
+	/*
+	 * ADC IN5 measurement
+	 */
+	uint16_t	adc_in5;
+
+	/*
+	 * Power supply overcurrent status bits.
+	 */
+	uint8_t		overcurrent;
+
 };
 
 extern struct sys_state_s system_state;
 
-extern int frame_rx;
-extern int frame_bad;
-
+#if 0
 /*
  * Software countdown timers.
  *
@@ -127,6 +135,7 @@ extern int frame_bad;
 #define TIMER_SANITY		7
 #define TIMER_NUM_TIMERS	8
 extern volatile int	timers[TIMER_NUM_TIMERS];
+#endif
 
 /*
  * GPIO handling.
@@ -141,10 +150,13 @@ extern volatile int	timers[TIMER_NUM_TIMERS];
 #define POWER_RELAY1(_s)	stm32_gpiowrite(GPIO_RELAY1_EN, (_s))
 #define POWER_RELAY2(_s)	stm32_gpiowrite(GPIO_RELAY2_EN, (_s))
 
-#define OVERCURRENT_ACC		stm32_gpioread(GPIO_ACC_OC_DETECT)
-#define OVERCURRENT_SERVO	stm32_gpioread(GPIO_SERVO_OC_DETECT
+#define OVERCURRENT_ACC		(!stm32_gpioread(GPIO_ACC_OC_DETECT))
+#define OVERCURRENT_SERVO	(!stm32_gpioread(GPIO_SERVO_OC_DETECT))
 #define BUTTON_SAFETY		stm32_gpioread(GPIO_BTN_SAFETY)
 
+#define ADC_VBATT		4
+#define ADC_IN5			5
+
 /*
  * Mixer
  */