AP_IOMCU: added analog input to iofirmware

libraries/AP_IOMCU/iofirmware/analog.cpp

@@ -0,0 +1,88 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   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
+ */
+
+#include "ch.h"
+#include "hal.h"
+#include "analog.h"
+
+#if HAL_USE_ADC != TRUE
+#error "HAL_USE_ADC must be set"
+#endif
+
+/*
+  initialise ADC capture
+ */
+void adc_init(void)
+{
+    adc_lld_init();
+    rccEnableADC1(true);
+
+    /* 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 */
+    ADC1->SQR1 = 0;
+    ADC1->SQR2 = 0;
+}
+
+/*
+  capture one sample on a channel
+ */
+static uint16_t adc_sample_channel(uint32_t channel)
+{
+    // clear EOC
+    ADC1->SR = 0;
+
+    /* capture one sample */
+    ADC1->SQR3 = channel;
+	ADC1->CR2 |= ADC_CR2_ADON;
+
+	/* wait for the conversion to complete */
+    uint32_t counter = 16;
+
+    while (!(ADC1->SR & ADC_SR_EOC)) {
+        if (--counter == 0) {
+            // ensure EOC is clear
+            ADC1->SR = 0;
+            return 0xffff;
+		}
+	}
+
+    // return sample (this also clears EOC flag)
+    return ADC1->DR;
+}
+
+/*
+  capture VSERVO in mV
+ */
+uint16_t adc_sample_vservo(void)
+{
+    const uint32_t channel = ADC_SQR3_SQ1_N(ADC_CHANNEL_IN4);
+    return adc_sample_channel(channel) * 9900 / 4096;
+}
+
+/*
+  capture VRSSI in mV
+ */
+uint16_t adc_sample_vrssi(void)
+{
+    const uint32_t channel = ADC_SQR3_SQ1_N(ADC_CHANNEL_IN5);
+    return adc_sample_channel(channel) * 9900 / 4096;
+}

libraries/AP_IOMCU/iofirmware/analog.h

@@ -0,0 +1,14 @@
+/*
+  initialise adc
+ */
+void adc_init(void);
+
+/*
+  capture VSERVO
+ */
+uint16_t adc_sample_vservo(void);
+
+/*
+  capture VRSSI
+ */
+uint16_t adc_sample_vrssi(void);

libraries/AP_IOMCU/iofirmware/iofirmware.cpp

@@ -1,5 +1,20 @@
-//IO Controller Firmware
-
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/*
+  IOMCU main firmware
+ */
 #include <AP_HAL/AP_HAL.h>
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
@@ -9,6 +24,7 @@
 #include "iofirmware.h"
 #include "hal.h"
 #include <AP_HAL_ChibiOS/RCInput.h>
+#include "analog.h"
 extern const AP_HAL::HAL &hal;
 
 //#pragma GCC optimize("Og")
@@ -139,6 +155,8 @@ void AP_IOMCU_FW::init()
     if (palReadLine(HAL_GPIO_PIN_IO_HW_DETECT1) == 1 && palReadLine(HAL_GPIO_PIN_IO_HW_DETECT2) == 0) {
         has_heater = true;
     }
+
+    adc_init();
 }
 
 
@@ -267,6 +285,15 @@ void AP_IOMCU_FW::process_io_packet()
     }
 }
 
+/*
+  update dynamic elements of status page
+ */
+void AP_IOMCU_FW::page_status_update(void)
+{
+    reg_status.vrssi = adc_sample_vrssi();
+    reg_status.vservo = adc_sample_vservo();
+}
+
 bool AP_IOMCU_FW::handle_code_read()
 {
     uint16_t *values = nullptr;
@@ -284,6 +311,7 @@ bool AP_IOMCU_FW::handle_code_read()
         COPY_PAGE(rc_input);
         break;
     case PAGE_STATUS:
+        page_status_update();
         COPY_PAGE(reg_status);
         break;
     case PAGE_SERVOS:

libraries/AP_IOMCU/iofirmware/iofirmware.h

@@ -28,6 +28,7 @@ private:
     void schedule_reboot(uint32_t time_ms);
     void safety_update();
     void rcout_mode_update();
+    void page_status_update(void);
 
     struct PACKED {
         /* default to RSSI ADC functionality */