AP_IOMCU: make adc interrupt driven

libraries/AP_IOMCU/iofirmware/analog.cpp

@@ -13,9 +13,9 @@
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 /*
-  analog capture for IOMCU. This uses direct register access to avoid
-  using up a DMA channel and to minimise latency. We capture a single
-  sample at a time
+   analog capture for IOMCU. This uses direct register access to avoid
+   using up a DMA channel and to minimise latency. We capture a single
+   sample at a time
  */
 
 #include "ch.h"
@@ -25,64 +25,135 @@
 #if HAL_USE_ADC != TRUE
 #error "HAL_USE_ADC must be set"
 #endif
+// we build this file with optimisation to lower the interrupt
+// latency.
+#pragma GCC optimize("O2")
+
+extern "C" {
+    extern void Vector88();
+}
+
+#define STM32_ADC1_NUMBER 18
+#define STM32_ADC1_HANDLER Vector88
+
+const uint32_t VSERVO_CHANNEL = ADC_SQR3_SQ1_N(ADC_CHANNEL_IN4);
+const uint32_t VRSSI_CHANNEL = ADC_SQR3_SQ1_N(ADC_CHANNEL_IN5);
+
+static uint16_t vrssi_val = 0xFFFF;
+static uint16_t vservo_val = 0xFFFF;
+static bool sample_vrssi_enable = true;
+static bool sampling_vservo = true;
 
 /*
   initialise ADC capture
  */
 void adc_init(void)
 {
-    adc_lld_init();
     rccEnableADC1(true);
+    ADC1->CR1 = 0;
+    ADC1->CR2 = ADC_CR2_ADON;
+
+    /* Reset calibration just to be safe.*/
+    ADC1->CR2 = ADC_CR2_ADON | ADC_CR2_RSTCAL;
+    while ((ADC1->CR2 & ADC_CR2_RSTCAL) != 0)
+      ;
+
+    /* Calibration.*/
+    ADC1->CR2 = ADC_CR2_ADON | ADC_CR2_CAL;
+    while ((ADC1->CR2 & ADC_CR2_CAL) != 0)
+      ;
 
     /* set channels 4 and 5 for 28.5us sample time */
     ADC1->SMPR2 = ADC_SMPR2_SMP_AN4(ADC_SAMPLE_28P5) | ADC_SMPR2_SMP_AN5(ADC_SAMPLE_28P5);
 
-    /* capture a single sample at a time */
+    /* capture one sample at a time */
     ADC1->SQR1 = 0;
     ADC1->SQR2 = 0;
+
+    ADC1->CR1 |= ADC_CR1_EOCIE;
+
+    nvicEnableVector(STM32_ADC1_NUMBER, STM32_ADC_ADC1_IRQ_PRIORITY);
 }
 
 /*
-  capture one sample on a channel
+  capture VSERVO in mV
  */
-static uint16_t adc_sample_channel(uint32_t channel)
+void adc_enable_vrssi(void)
 {
-    // clear EOC
-    ADC1->SR = 0;
+    sample_vrssi_enable = true;
+}
 
-    /* capture one sample */
-    ADC1->SQR3 = channel;
-    ADC1->CR2 |= ADC_CR2_ADON;
+/*
+  don't capture VRSSI
+ */
+void adc_disable_vrssi(void)
+{
+    sample_vrssi_enable = false;
+}
 
-    /* wait for the conversion to complete */
-    uint32_t counter = 16;
+/*
+  capture one sample on a channel
+ */
+void adc_sample_channels()
+{
+    chSysLock();
 
-    while (!(ADC1->SR & ADC_SR_EOC)) {
-        if (--counter == 0) {
-            // ensure EOC is clear
-            ADC1->SR = 0;
-            return 0xffff;
-        }
+    if (ADC1->SR & ADC_SR_STRT) {
+        return; // still waiting for sample
     }
 
-    // return sample (this also clears EOC flag)
-    return ADC1->DR;
+    /* capture another sample */
+    ADC1->CR2 |= ADC_CR2_ADON;
+
+    chSysUnlock();
 }
 
 /*
   capture VSERVO in mV
  */
-uint16_t adc_sample_vservo(void)
+uint16_t adc_vservo(void)
 {
-    const uint32_t channel = ADC_SQR3_SQ1_N(ADC_CHANNEL_IN4);
-    return adc_sample_channel(channel);
+    return vservo_val;
 }
 
 /*
   capture VRSSI in mV
  */
-uint16_t adc_sample_vrssi(void)
+uint16_t adc_vrssi(void)
 {
-    const uint32_t channel = ADC_SQR3_SQ1_N(ADC_CHANNEL_IN5);
-    return adc_sample_channel(channel);
+    return vrssi_val;
+}
+
+static void adc_read_sample()
+{
+    if (ADC1->SR & ADC_SR_EOC) {
+
+        ADC1->SR &= ~(ADC_SR_EOC | ADC_SR_STRT);
+
+        if (sampling_vservo) {
+            vservo_val = ADC1->DR;
+            if (sample_vrssi_enable) {
+                /* capture another sample */
+                ADC1->SQR3 = VRSSI_CHANNEL;
+                ADC1->CR2 |= ADC_CR2_ADON;
+                sampling_vservo = false;
+            }
+        } else {
+            vrssi_val = ADC1->DR;
+            ADC1->SQR3 = VSERVO_CHANNEL;
+            sampling_vservo = true;
+        }
+    }
+}
+
+OSAL_IRQ_HANDLER(STM32_ADC1_HANDLER) {
+    OSAL_IRQ_PROLOGUE();
+
+    chSysLockFromISR();
+
+    adc_read_sample();
+
+    chSysUnlockFromISR();
+
+    OSAL_IRQ_EPILOGUE();
 }

libraries/AP_IOMCU/iofirmware/analog.h

@@ -9,9 +9,18 @@ void adc_init(void);
 /*
   capture VSERVO
  */
-uint16_t adc_sample_vservo(void);
+uint16_t adc_vservo(void);
 
 /*
   capture VRSSI
  */
-uint16_t adc_sample_vrssi(void);
+uint16_t adc_vrssi(void);
+
+/* start another update */
+void adc_sample_channels(void);
+
+/* capture VRSSI */
+void adc_enable_vrssi(void);
+
+/* don't capture VRSSI */
+void adc_disable_vrssi(void);

libraries/AP_IOMCU/iofirmware/iofirmware.cpp

@@ -671,13 +671,15 @@ void AP_IOMCU_FW::process_io_packet()
  */
 void AP_IOMCU_FW::page_status_update(void)
 {
+    adc_sample_channels();
+
     if ((reg_setup.features & P_SETUP_FEATURES_SBUS1_OUT) == 0) {
         // we can only get VRSSI when sbus is disabled
-        reg_status.vrssi = adc_sample_vrssi();
+        reg_status.vrssi = adc_vrssi();
     } else {
         reg_status.vrssi = 0;
     }
-    reg_status.vservo = adc_sample_vservo();
+    reg_status.vservo = adc_vservo();
 }
 
 bool AP_IOMCU_FW::handle_code_read()
@@ -798,8 +800,10 @@ bool AP_IOMCU_FW::handle_code_write()
                 // or disable SBUS out
                 AFIO->MAPR = AFIO_MAPR_SWJ_CFG_NOJNTRST;
 
+                adc_disable_vrssi();
                 palClearLine(HAL_GPIO_PIN_SBUS_OUT_EN);
             } else {
+                adc_enable_vrssi();
                 palSetLine(HAL_GPIO_PIN_SBUS_OUT_EN);
             }
             if (reg_setup.features & P_SETUP_FEATURES_HEATER) {