mirror of https://github.com/ArduPilot/ardupilot
370 lines
10 KiB
C++
370 lines
10 KiB
C++
/*
|
|
* This file 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 file 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/>.
|
|
*
|
|
* Code by Charles Villard
|
|
*/
|
|
#include <AP_HAL/AP_HAL.h>
|
|
#include <AP_HAL_ESP32/Semaphores.h>
|
|
|
|
#include <stdlib.h>
|
|
#include <stdio.h>
|
|
|
|
#include "freertos/FreeRTOS.h"
|
|
#include "freertos/task.h"
|
|
#include "driver/gpio.h"
|
|
#include "driver/adc.h"
|
|
#include "esp_adc_cal.h"
|
|
#include "soc/adc_channel.h"
|
|
|
|
#if HAL_USE_ADC == TRUE && !defined(HAL_DISABLE_ADC_DRIVER)
|
|
|
|
#include "AnalogIn.h"
|
|
|
|
#ifndef ESP32_ADC_MAVLINK_DEBUG
|
|
// this allows the first 6 analog channels to be reported by mavlink for debugging purposes
|
|
#define ESP32_ADC_MAVLINK_DEBUG 0
|
|
#endif
|
|
|
|
#include <GCS_MAVLink/GCS_MAVLink.h>
|
|
|
|
#define ANALOGIN_DEBUGGING 0
|
|
|
|
// base voltage scaling for 12 bit 3.3V ADC
|
|
#define VOLTAGE_SCALING (3.3f/4096.0f)
|
|
|
|
#if ANALOGIN_DEBUGGING
|
|
# define Debug(fmt, args ...) do {printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
|
|
#else
|
|
# define Debug(fmt, args ...)
|
|
#endif
|
|
|
|
// we are limited to using adc1, and it supports 8 channels max, on gpio, in this order:
|
|
// ADC1_CH0=D36,ADC1_CH1=D37,ADC1_CH2=D38,ADC1_CH3=D39,ADC1_CH4=D32,ADC1_CH5=D33,ADC1_CH6=D34,ADC1_CH7=D35
|
|
// this driver will only configure the ADCs from a subset of these that the board exposes on pins.
|
|
|
|
|
|
extern const AP_HAL::HAL &hal;
|
|
|
|
using namespace ESP32;
|
|
|
|
/*
|
|
scaling table between ADC count and actual input voltage, to account
|
|
for voltage dividers on the board.
|
|
*/
|
|
const AnalogIn::pin_info AnalogIn::pin_config[] = HAL_ESP32_ADC_PINS;
|
|
|
|
#define ADC_GRP1_NUM_CHANNELS ARRAY_SIZE(AnalogIn::pin_config)
|
|
|
|
|
|
#define DEFAULT_VREF 1100 //Use adc2_vref_to_gpio() to obtain a better estimate
|
|
#define NO_OF_SAMPLES 256 //Multisampling
|
|
|
|
static const adc_atten_t atten = ADC_ATTEN_DB_11;
|
|
|
|
//ardupin is the ardupilot assigned number, starting from 1-8(max)
|
|
// 'pin' and _pin is a macro like 'ADC1_GPIO35_CHANNEL' from board config .h
|
|
AnalogSource::AnalogSource(int16_t ardupin,int16_t pin,float scaler, float initial_value, uint8_t unit) :
|
|
|
|
_unit(unit),
|
|
_ardupin(ardupin),
|
|
_pin(pin),
|
|
_scaler(scaler),
|
|
_value(initial_value),
|
|
_latest_value(initial_value),
|
|
_sum_count(0),
|
|
_sum_value(0)
|
|
{
|
|
printf("AnalogIn: adding ardupin:%d-> which is adc1_offset:%d\n", _ardupin,_pin);
|
|
|
|
// init the pin now if possible, otherwise doo it later from set_pin
|
|
if ( _ardupin != ANALOG_INPUT_NONE ) {
|
|
|
|
// dertermine actial gpio from adc offset and configure it
|
|
gpio_num_t gpio;
|
|
//Configure ADC
|
|
if (unit == 1) {
|
|
adc1_config_channel_atten((adc1_channel_t)_pin, atten);
|
|
adc1_pad_get_io_num((adc1_channel_t)_pin, &gpio);
|
|
} else {
|
|
adc2_config_channel_atten((adc2_channel_t)_pin, atten);
|
|
}
|
|
|
|
esp_adc_cal_characteristics_t adc_chars;
|
|
esp_adc_cal_characterize(ADC_UNIT_1, atten, ADC_WIDTH_BIT_12, DEFAULT_VREF, &adc_chars);
|
|
printf("AnalogIn: determined actual gpio as: %d\n", gpio);
|
|
|
|
_gpio = gpio;// save it for later
|
|
|
|
}
|
|
}
|
|
|
|
|
|
float AnalogSource::read_average()
|
|
{
|
|
if ( _ardupin == ANALOG_INPUT_NONE ) {
|
|
return 0.0f;
|
|
}
|
|
|
|
WITH_SEMAPHORE(_semaphore);
|
|
|
|
if (_sum_count == 0) {
|
|
uint32_t adc_reading = 0;
|
|
//Multisampling
|
|
for (int i = 0; i < NO_OF_SAMPLES; i++) {
|
|
if (_unit == 1) {
|
|
adc_reading += adc1_get_raw((adc1_channel_t)_pin);
|
|
} else {
|
|
int raw;
|
|
adc2_get_raw((adc2_channel_t)_pin, ADC_WIDTH_BIT_12, &raw);
|
|
adc_reading += raw;
|
|
}
|
|
}
|
|
adc_reading /= NO_OF_SAMPLES;
|
|
return adc_reading;
|
|
}
|
|
|
|
_value = _sum_value / _sum_count;
|
|
_sum_value = 0;
|
|
_sum_count = 0;
|
|
|
|
return _value;
|
|
}
|
|
|
|
float AnalogSource::read_latest()
|
|
{
|
|
return _latest_value;
|
|
}
|
|
|
|
//_scaler scaling from ADC count to Volts
|
|
|
|
/*
|
|
return voltage in Volts
|
|
*/
|
|
float AnalogSource::voltage_average()
|
|
{
|
|
return _scaler * read_average();
|
|
}
|
|
|
|
/*
|
|
return voltage in Volts
|
|
*/
|
|
float AnalogSource::voltage_latest()
|
|
{
|
|
return _scaler * read_latest();
|
|
}
|
|
|
|
float AnalogSource::voltage_average_ratiometric()
|
|
{
|
|
return _scaler * read_latest();
|
|
}
|
|
|
|
// ardupin
|
|
bool AnalogSource::set_pin(uint8_t ardupin)
|
|
{
|
|
|
|
if (_ardupin == ardupin) {
|
|
return true;
|
|
}
|
|
|
|
int8_t pinconfig_offset = AnalogIn::find_pinconfig(ardupin);
|
|
if (pinconfig_offset == -1 ) {
|
|
DEV_PRINTF("AnalogIn: sorry set_pin() can't determine ADC1 offset from ardupin : %d \n",ardupin);
|
|
return false;
|
|
}
|
|
|
|
int16_t newgpioAdcPin = AnalogIn::pin_config[(uint8_t)pinconfig_offset].channel;
|
|
float newscaler = AnalogIn::pin_config[(uint8_t)pinconfig_offset].scaling;
|
|
|
|
if (_pin == newgpioAdcPin) {
|
|
return true;
|
|
}
|
|
|
|
WITH_SEMAPHORE(_semaphore);
|
|
|
|
// init the target pin now if possible
|
|
if ( ardupin != ANALOG_INPUT_NONE ) {
|
|
|
|
gpio_num_t gpio; // new gpio
|
|
//Configure ADC
|
|
if (_unit == 1) {
|
|
adc1_config_channel_atten((adc1_channel_t)newgpioAdcPin, atten);
|
|
adc1_pad_get_io_num((adc1_channel_t)newgpioAdcPin, &gpio);
|
|
} else {
|
|
adc2_config_channel_atten((adc2_channel_t)newgpioAdcPin, atten);
|
|
}
|
|
|
|
esp_adc_cal_characteristics_t adc_chars;
|
|
esp_adc_cal_characterize(ADC_UNIT_1, atten, ADC_WIDTH_BIT_12, DEFAULT_VREF, &adc_chars);
|
|
printf("AnalogIn: Adding gpio on: %d\n", gpio);
|
|
|
|
DEV_PRINTF("AnalogIn: set_pin() FROM (ardupin:%d adc1_offset:%d gpio:%d) TO (ardupin:%d adc1_offset:%d gpio:%d)\n", \
|
|
_ardupin,_pin,_gpio,ardupin,newgpioAdcPin,gpio);
|
|
_pin = newgpioAdcPin;
|
|
_ardupin = ardupin;
|
|
_gpio = gpio;
|
|
_scaler = newscaler;
|
|
|
|
}
|
|
|
|
_sum_value = 0;
|
|
_sum_count = 0;
|
|
_latest_value = 0;
|
|
_value = 0;
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
/*
|
|
apply a reading in ADC counts
|
|
*/
|
|
void AnalogSource::_add_value()
|
|
{
|
|
if ( _ardupin == ANALOG_INPUT_NONE ) {
|
|
return;
|
|
}
|
|
|
|
WITH_SEMAPHORE(_semaphore);
|
|
|
|
int value = 0;
|
|
if (_unit == 1) {
|
|
value = adc1_get_raw((adc1_channel_t)_pin);
|
|
} else {
|
|
adc2_get_raw((adc2_channel_t)_pin, ADC_WIDTH_BIT_12, &value);
|
|
}
|
|
|
|
_latest_value = value;
|
|
_sum_value += value;
|
|
_sum_count++;
|
|
|
|
if (_sum_count == 254) {
|
|
_sum_value /= 2;
|
|
_sum_count /= 2;
|
|
}
|
|
}
|
|
|
|
static void check_efuse()
|
|
{
|
|
//Check TP is burned into eFuse
|
|
if (esp_adc_cal_check_efuse(ESP_ADC_CAL_VAL_EFUSE_TP) == ESP_OK) {
|
|
printf("AnalogIn: eFuse Two Point: Supported\n");
|
|
} else {
|
|
printf("AnalogIn: eFuse Two Point: NOT supported\n");
|
|
}
|
|
|
|
//Check Vref is burned into eFuse
|
|
if (esp_adc_cal_check_efuse(ESP_ADC_CAL_VAL_EFUSE_VREF) == ESP_OK) {
|
|
printf("AnalogIn: eFuse Vref: Supported\n");
|
|
} else {
|
|
printf("AnalogIn: eFuse Vref: NOT supported\n");
|
|
}
|
|
}
|
|
|
|
/*
|
|
setup adc peripheral to capture samples with DMA into a buffer
|
|
*/
|
|
void AnalogIn::init()
|
|
{
|
|
check_efuse();
|
|
|
|
adc1_config_width(ADC_WIDTH_BIT_12);
|
|
}
|
|
|
|
/*
|
|
called at 1kHz
|
|
*/
|
|
void AnalogIn::_timer_tick()
|
|
{
|
|
for (uint8_t j = 0; j < ANALOG_MAX_CHANNELS; j++) {
|
|
ESP32::AnalogSource *c = _channels[j];
|
|
if (c != nullptr) {
|
|
// add a value
|
|
//c->_add_value();
|
|
}
|
|
}
|
|
|
|
#if ESP32_ADC_MAVLINK_DEBUG
|
|
static uint8_t count;
|
|
if (AP_HAL::millis() > 5000 && count++ == 10) {
|
|
count = 0;
|
|
uint16_t adc[6] {};
|
|
uint8_t n = ADC_GRP1_NUM_CHANNELS;
|
|
if (n > 6) {
|
|
n = 6;
|
|
}
|
|
for (uint8_t i = 0; i < n; i++) {
|
|
adc[i] = buf_adc[i];
|
|
}
|
|
mavlink_msg_ap_adc_send(MAVLINK_COMM_0, adc[0], adc[1], adc[2], adc[3], adc[4],
|
|
adc[5]);
|
|
}
|
|
#endif
|
|
|
|
}
|
|
|
|
//positive array index (zero is ok), or -1 on error
|
|
int8_t AnalogIn::find_pinconfig(int16_t ardupin)
|
|
{
|
|
// from ardupin, lookup which adc gpio that is..
|
|
for (uint8_t j = 0; j < ADC_GRP1_NUM_CHANNELS; j++) {
|
|
if (pin_config[j].ardupin == ardupin) {
|
|
return j;
|
|
}
|
|
}
|
|
// can't find a match in definitons
|
|
return -1;
|
|
|
|
}
|
|
|
|
//
|
|
AP_HAL::AnalogSource *AnalogIn::channel(int16_t ardupin)
|
|
{
|
|
int8_t pinconfig_offset = find_pinconfig(ardupin);
|
|
|
|
int16_t gpioAdcPin = -1;
|
|
float scaler = -1;
|
|
|
|
if ((ardupin != ANALOG_INPUT_NONE) && (pinconfig_offset == -1 )) {
|
|
DEV_PRINTF("AnalogIn: sorry channel() can't determine ADC1 offset from ardupin : %d \n",ardupin);
|
|
ardupin = ANALOG_INPUT_NONE; // default it to this not terrible value and allow to continue
|
|
}
|
|
// although ANALOG_INPUT_NONE=255 is not a valid pin, we let it through here as
|
|
// a special case, so that it can be changed with set_pin(..) later.
|
|
if (ardupin != ANALOG_INPUT_NONE) {
|
|
gpioAdcPin = pin_config[(uint8_t)pinconfig_offset].channel;
|
|
scaler = pin_config[(uint8_t)pinconfig_offset].scaling;
|
|
}
|
|
|
|
for (uint8_t j = 0; j < ANALOG_MAX_CHANNELS; j++) {
|
|
if (_channels[j] == nullptr) {
|
|
_channels[j] = new AnalogSource(ardupin,gpioAdcPin, scaler,0.0f,1);
|
|
|
|
if (ardupin != ANALOG_INPUT_NONE) {
|
|
DEV_PRINTF("AnalogIn: channel:%d attached to ardupin:%d at adc1_offset:%d on gpio:%d\n",\
|
|
j,ardupin, gpioAdcPin, _channels[j]->_gpio);
|
|
}
|
|
|
|
if (ardupin == ANALOG_INPUT_NONE) {
|
|
DEV_PRINTF("AnalogIn: channel:%d created but using delayed adc and gpio pin configuration\n",j );
|
|
}
|
|
|
|
return _channels[j];
|
|
}
|
|
}
|
|
DEV_PRINTF("AnalogIn: out of channels\n");
|
|
return nullptr;
|
|
}
|
|
|
|
#endif // HAL_USE_ADC
|