mirror of https://github.com/ArduPilot/ardupilot
160 lines
3.4 KiB
C++
160 lines
3.4 KiB
C++
/*
|
|
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
|
|
// 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)
|
|
{
|
|
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 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 VSERVO in mV
|
|
*/
|
|
void adc_enable_vrssi(void)
|
|
{
|
|
sample_vrssi_enable = true;
|
|
}
|
|
|
|
/*
|
|
don't capture VRSSI
|
|
*/
|
|
void adc_disable_vrssi(void)
|
|
{
|
|
sample_vrssi_enable = false;
|
|
}
|
|
|
|
/*
|
|
capture one sample on a channel
|
|
*/
|
|
void adc_sample_channels()
|
|
{
|
|
chSysLock();
|
|
|
|
if (ADC1->SR & ADC_SR_STRT) {
|
|
return; // still waiting for sample
|
|
}
|
|
|
|
/* capture another sample */
|
|
ADC1->CR2 |= ADC_CR2_ADON;
|
|
|
|
chSysUnlock();
|
|
}
|
|
|
|
/*
|
|
capture VSERVO in mV
|
|
*/
|
|
uint16_t adc_vservo(void)
|
|
{
|
|
return vservo_val;
|
|
}
|
|
|
|
/*
|
|
capture VRSSI in mV
|
|
*/
|
|
uint16_t adc_vrssi(void)
|
|
{
|
|
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();
|
|
}
|