1 files changed, 37 insertions, 2 deletions

diff --git a/firmware/drivers/adc.c b/firmware/drivers/adc.c
index 861be80987..2bb5edf3bf 100644
--- a/firmware/drivers/adc.c
+++ b/firmware/drivers/adc.c
@@ -48,10 +48,45 @@ unsigned short adc_read(int channel)
     return adcdata[channel];
 }
 
+/* Batch convert 4 analog channels. If lower is true, convert AN0-AN3, 
+ * otherwise AN4-AN7. 
+ */
+static void adc_batch_convert(bool lower)
+{
+    int reg = lower ? 0 : 4;
+    volatile unsigned short* ANx = ((unsigned short*) ADDRAH_ADDR);
+    int i;
+
+    ADCSR = ADCSR_ADST | ADCSR_SCAN | ADCSR_CKS | reg | 3;
+
+    /* Busy wait until conversion is complete. A bit ugly perhaps, but 
+     * we should only need to wait about 4 * 14 µs  */
+    while(!(ADCSR & ADCSR_ADF))
+    {
+    }
+
+    /* Stop scanning */
+    ADCSR = 0;
+
+    for (i = 0; i < 4; i++)
+    {
+        /* Read converted values */
+        adcdata[reg++] = ANx[i] >> 6;
+    }
+}
+
 void adc_init(void)
 {
-    ADCR  = 0; /* No external trigger */
-    
+    int i;
+    ADCR  = 0x7f; /* No external trigger; other bits should be 1 according to the manual... */
+
     current_channel = 0;
+
+    /* Do a first scan to initialize all values */
+    /* AN0 to AN3 */
+    adc_batch_convert(true);
+    /* AN4 to AN7 */
+    adc_batch_convert(false);
+    
     tick_add_task(adc_tick);
 }