351 lines
11 KiB
C++
351 lines
11 KiB
C++
/*
|
|
* Copyright (C) EdgeTx
|
|
*
|
|
* Based on code named
|
|
* opentx - https://github.com/opentx/opentx
|
|
* th9x - http://code.google.com/p/th9x
|
|
* er9x - http://code.google.com/p/er9x
|
|
* gruvin9x - http://code.google.com/p/gruvin9x
|
|
*
|
|
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
|
|
*
|
|
* This program is free software; you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License version 2 as
|
|
* published by the Free Software Foundation.
|
|
*
|
|
* 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.
|
|
*/
|
|
|
|
#include "adc_driver.h"
|
|
#include "board.h"
|
|
|
|
// IS_POT_SLIDER_AVAILABLE()
|
|
#include "opentx.h"
|
|
|
|
const etx_hal_adc_driver_t* etx_hal_adc_driver = nullptr;
|
|
|
|
#if defined(SIMU)
|
|
// not needed
|
|
#elif defined(RADIO_T16)
|
|
const int8_t adcDirection[NUM_ANALOGS] = {1,-1,1,-1, 1,1,1, -1,1,1,1, -1,1 };
|
|
#elif defined(RADIO_T18)
|
|
const int8_t adcDirection[NUM_ANALOGS] = {1,-1,1,-1, -1,1,-1, -1,1,1,1, -1,1 };
|
|
#elif defined(RADIO_TX16S)
|
|
const int8_t adcDirection[NUM_ANALOGS] = {1,-1,1,-1, 1,1,1, -1,1,1,1, -1,1 };
|
|
#elif defined(PCBX10)
|
|
const int8_t adcDirection[NUM_ANALOGS] = {1,-1,1,-1, -1,1,-1, 1,1,1,1, 1,-1 };
|
|
#elif defined(PCBX9E)
|
|
const int8_t adcDirection[NUM_ANALOGS] = {
|
|
#if defined(HORUS_STICKS)
|
|
1,-1,1,-1,
|
|
#else
|
|
1,1,-1,-1,
|
|
#endif // HORUS_STICKS
|
|
// other analogs are the same
|
|
-1,-1,-1,1, -1,1,1,-1, -1,-1 };
|
|
|
|
const uint8_t adcMapping[NUM_ANALOGS] = { 0 /*STICK1*/, 1 /*STICK2*/, 2 /*STICK3*/, 3 /*STICK4*/,
|
|
11 /*POT1*/, 4 /*POT2*/, 5 /*POT3*/, 6 /*POT4*/,
|
|
12 /*SLIDER1*/, 13 /*SLIDER2*/, 7 /*SLIDER3*/, 8 /*SLIDER4*/,
|
|
9 /*TX_VOLTAGE*/, 10 /*TX_VBAT*/ };
|
|
#elif defined(PCBX9DP)
|
|
const int8_t adcDirection[NUM_ANALOGS] = {1,-1,1,-1, 1,1,-1, 1,1, 1, 1};
|
|
#elif defined(PCBX9D)
|
|
const int8_t adcDirection[NUM_ANALOGS] = {1,-1,1,-1, 1,1,0, 1,1, 1, 1};
|
|
#elif defined(RADIO_TX12)
|
|
const int8_t adcDirection[NUM_ANALOGS] = {-1,1,-1,1, -1,-1, 1, 1};
|
|
#elif defined(RADIO_ZORRO)
|
|
const int8_t adcDirection[NUM_ANALOGS] = {-1, 1, 1, -1, -1, 1, 1, 1};
|
|
#elif defined(RADIO_TPRO)
|
|
const int8_t adcDirection[NUM_ANALOGS] = {1,-1,1,-1, 1,1, 1, 1};
|
|
#elif defined(PCBX7)
|
|
const int8_t adcDirection[NUM_ANALOGS] = {-1,1,-1,1, 1,1, 1, 1};
|
|
#elif defined(PCBX9LITE)
|
|
const int8_t adcDirection[NUM_ANALOGS] = {-1,1,-1,1, 1,1, 1};
|
|
#elif defined(PCBXLITE)
|
|
const int8_t adcDirection[NUM_ANALOGS] = {1,-1,-1,1, -1,1, 1, 1};
|
|
#elif defined(PCBNV14)
|
|
const uint8_t adcMapping[NUM_ANALOGS] = { 0 /*STICK1*/, 1 /*STICK2*/, 2 /*STICK3*/, 3 /*STICK4*/,
|
|
4 /*POT1*/, 5 /*POT2*/, 6 /*SWA*/, 14 /*SWB*/,
|
|
7 /*SWC*/, 15 /*SWD*/, 8 /*SWE*/, 9 /*SWF*/,
|
|
11/*SWG*/, 10/*SWH*/,
|
|
12 /*TX_VOLTAGE*/, 13 /* TX_VBAT */ };
|
|
|
|
const int8_t adcDirection[NUM_ANALOGS] = { 0 /*STICK1*/, 0 /*STICK2*/, 0 /*STICK3*/, 0 /*STICK4*/,
|
|
-1 /*POT1*/, 0 /*POT2*/, 0 /*SWA*/, 0 /*SWC*/,
|
|
0 /*SWE*/, -1 /*SWF*/, 0 /*SWG*/, -1 /*SWH*/,
|
|
0 /*TX_VOLTAGE*/, 0 /*TX_VBAT*/,
|
|
0 /*SWB*/, 0 /*SWD*/};
|
|
|
|
#elif defined(PCBX12S)
|
|
const int8_t adcDirection[NUM_ANALOGS] = {1,-1,1,-1, -1,1,-1, -1,-1, -1,1, 0,0,0};
|
|
#else
|
|
#error "ADC driver does not suppport this target"
|
|
#endif
|
|
|
|
uint16_t adcValues[NUM_ANALOGS] __DMA;
|
|
|
|
#if defined(PCBX10) || defined(PCBX12S)
|
|
uint16_t rtcBatteryVoltage;
|
|
#endif
|
|
|
|
bool adcInit(const etx_hal_adc_driver_t* driver)
|
|
{
|
|
etx_hal_adc_driver = driver;
|
|
if (!etx_hal_adc_driver)
|
|
return false;
|
|
|
|
// If there is an init function
|
|
// it should succeed
|
|
if (etx_hal_adc_driver->init &&
|
|
!etx_hal_adc_driver->init()) {
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
static bool adcSingleRead()
|
|
{
|
|
if (!etx_hal_adc_driver)
|
|
return false;
|
|
|
|
if (etx_hal_adc_driver->start_conversion &&
|
|
!etx_hal_adc_driver->start_conversion())
|
|
return false;
|
|
|
|
if (etx_hal_adc_driver->wait_completion)
|
|
etx_hal_adc_driver->wait_completion();
|
|
|
|
return true;
|
|
}
|
|
|
|
// Declare adcRead() weak so it can be re-declared
|
|
#pragma weak adcRead
|
|
bool adcRead()
|
|
{
|
|
uint16_t temp[NUM_ANALOGS] = { 0 };
|
|
|
|
uint8_t first_analog_adc;
|
|
#if defined(RADIO_FAMILY_T16) || defined(PCBNV14)
|
|
if (globalData.flyskygimbals)
|
|
{
|
|
first_analog_adc = FIRST_ANALOG_ADC_FS;
|
|
} else
|
|
{
|
|
first_analog_adc = FIRST_ANALOG_ADC;
|
|
}
|
|
#else
|
|
first_analog_adc = FIRST_ANALOG_ADC;
|
|
#endif
|
|
|
|
|
|
for (int i=0; i<4; i++) {
|
|
if (!adcSingleRead())
|
|
return false;
|
|
for (uint8_t x=first_analog_adc; x<NUM_ANALOGS; x++) {
|
|
uint16_t val = adcValues[x];
|
|
#if defined(JITTER_MEASURE)
|
|
if (JITTER_MEASURE_ACTIVE()) {
|
|
rawJitter[x].measure(val);
|
|
}
|
|
#endif
|
|
temp[x] += val;
|
|
}
|
|
}
|
|
|
|
for (uint8_t x=first_analog_adc; x<NUM_ANALOGS; x++) {
|
|
adcValues[x] = temp[x] >> 2;
|
|
}
|
|
|
|
#if NUM_PWMSTICKS > 0
|
|
if (STICKS_PWM_ENABLED()) {
|
|
sticksPwmRead(adcValues);
|
|
}
|
|
#endif
|
|
return true;
|
|
}
|
|
|
|
#if !defined(SIMU)
|
|
uint16_t getRTCBatteryVoltage()
|
|
{
|
|
#if defined(HAS_TX_RTC_VOLTAGE)
|
|
return (getAnalogValue(TX_RTC_VOLTAGE) * ADC_VREF_PREC2) / 2048;
|
|
#elif defined(PCBX10) || defined(PCBX12S)
|
|
return (rtcBatteryVoltage * 2 * ADC_VREF_PREC2) / 2048;
|
|
#elif defined(PCBNV14)
|
|
#warning "TODO RTC voltage"
|
|
return 330;
|
|
#else
|
|
#warning "RTC battery not supported on this target"
|
|
return 0;
|
|
#endif
|
|
}
|
|
|
|
uint16_t getAnalogValue(uint8_t index)
|
|
{
|
|
if (IS_POT(index) && !IS_POT_SLIDER_AVAILABLE(index)) {
|
|
// Use fixed analog value for non-existing and/or non-connected pots.
|
|
// Non-connected analog inputs will slightly follow the adjacent connected analog inputs,
|
|
// which produces ghost readings on these inputs.
|
|
return 0;
|
|
}
|
|
#if defined(PCBX9E) || defined(PCBNV14)
|
|
index = adcMapping[index];
|
|
#endif
|
|
if (adcDirection[index] < 0)
|
|
return ADCMAXVALUE - adcValues[index];
|
|
else
|
|
return adcValues[index];
|
|
}
|
|
|
|
static uint32_t s_anaFilt[NUM_ANALOGS];
|
|
|
|
#define JITTER_ALPHA (1<<JITTER_FILTER_STRENGTH)
|
|
#define ANALOG_MULTIPLIER (1<<ANALOG_SCALE)
|
|
#define ANA_FILT(chan) (s_anaFilt[chan] / (JITTER_ALPHA * ANALOG_MULTIPLIER))
|
|
#if (JITTER_ALPHA * ANALOG_MULTIPLIER > 32)
|
|
#error "JITTER_FILTER_STRENGTH and ANALOG_SCALE are too big, their summ should be <= 5 !!!"
|
|
#endif
|
|
|
|
uint16_t anaIn(uint8_t chan)
|
|
{
|
|
return ANA_FILT(chan);
|
|
}
|
|
|
|
#if defined(JITTER_MEASURE)
|
|
JitterMeter<uint16_t> rawJitter[NUM_ANALOGS];
|
|
JitterMeter<uint16_t> avgJitter[NUM_ANALOGS];
|
|
tmr10ms_t jitterResetTime = 0;
|
|
#endif
|
|
|
|
uint16_t getBatteryVoltage()
|
|
{
|
|
// using filtered ADC value on purpose
|
|
int32_t instant_vbat = anaIn(TX_VOLTAGE);
|
|
return (uint16_t)((instant_vbat * (1000 + g_eeGeneral.txVoltageCalibration)) /
|
|
BATTERY_DIVIDER);
|
|
}
|
|
|
|
extern uint16_t get_flysky_hall_adc_value(uint8_t ch);
|
|
|
|
void getADC()
|
|
{
|
|
#if defined(JITTER_MEASURE)
|
|
if (JITTER_MEASURE_ACTIVE() && jitterResetTime < get_tmr10ms()) {
|
|
// reset jitter measurement every second
|
|
for (uint32_t x=0; x<NUM_ANALOGS; x++) {
|
|
rawJitter[x].reset();
|
|
avgJitter[x].reset();
|
|
}
|
|
jitterResetTime = get_tmr10ms() + 100; //every second
|
|
}
|
|
#endif
|
|
|
|
DEBUG_TIMER_START(debugTimerAdcRead);
|
|
if (!adcRead())
|
|
TRACE("adcRead failed");
|
|
DEBUG_TIMER_STOP(debugTimerAdcRead);
|
|
|
|
for (uint8_t x=0; x<NUM_ANALOGS; x++) {
|
|
uint32_t v;
|
|
#if defined(RADIO_FAMILY_T16) || defined(PCBNV14)
|
|
if (globalData.flyskygimbals)
|
|
{
|
|
if (x < 4) {
|
|
v = get_flysky_hall_adc_value(x) >> (1 - ANALOG_SCALE);
|
|
} else {
|
|
v = getAnalogValue(x) >> (1 - ANALOG_SCALE);
|
|
}
|
|
}
|
|
else
|
|
#endif
|
|
{
|
|
v = getAnalogValue(x) >> (1 - ANALOG_SCALE);
|
|
}
|
|
|
|
// Jitter filter:
|
|
// * pass trough any big change directly
|
|
// * for small change use Modified moving average (MMA) filter
|
|
//
|
|
// Explanation:
|
|
//
|
|
// Normal MMA filter has this formula:
|
|
// <out> = ((ALPHA-1)*<out> + <in>)/ALPHA
|
|
//
|
|
// If calculation is done this way with integer arithmetics, then any small change in
|
|
// input signal is lost. One way to combat that, is to rearrange the formula somewhat,
|
|
// to store a more precise (larger) number between iterations. The basic idea is to
|
|
// store undivided value between iterations. Therefore an new variable <filtered> is
|
|
// used. The new formula becomes:
|
|
// <filtered> = <filtered> - <filtered>/ALPHA + <in>
|
|
// <out> = <filtered>/ALPHA (use only when out is needed)
|
|
//
|
|
// The above formula with a maximum allowed ALPHA value (we are limited by
|
|
// the 16 bit s_anaFilt[]) was tested on the radio. The resulting signal still had
|
|
// some jitter (a value of 1 was observed). The jitter might be bigger on other
|
|
// radios.
|
|
//
|
|
// So another idea is to use larger input values for filtering. So instead of using
|
|
// input in a range from 0 to 2047, we use twice larger number (temp[x] is divided less)
|
|
//
|
|
// This also means that ALPHA must be lowered (remember 16 bit limit), but test results
|
|
// have proved that this kind of filtering gives better results. So the recommended values
|
|
// for filter are:
|
|
// JITTER_FILTER_STRENGTH 4
|
|
// ANALOG_SCALE 1
|
|
//
|
|
// Variables mapping:
|
|
// * <in> = v
|
|
// * <out> = s_anaFilt[x]
|
|
uint32_t previous = s_anaFilt[x] / JITTER_ALPHA;
|
|
uint32_t diff = (v > previous) ? (v - previous) : (previous - v);
|
|
|
|
// Combine ADC jitter filter setting form radio and model.
|
|
// Model can override (on or off) or use setting from radio setup.
|
|
// Model setting is active when 1, radio setting is active when 0
|
|
uint8_t useJitterFilter = 0;
|
|
if (g_model.jitterFilter == OVERRIDE_GLOBAL) {
|
|
// Use radio setting - which is inverted
|
|
useJitterFilter = !g_eeGeneral.noJitterFilter;
|
|
} else {
|
|
// Enable if value is "On", disable if "Off"
|
|
useJitterFilter = (g_model.jitterFilter == OVERRIDE_ON)?1:0;
|
|
}
|
|
|
|
if (useJitterFilter && diff < (10*ANALOG_MULTIPLIER)) {
|
|
// apply jitter filter
|
|
s_anaFilt[x] = (s_anaFilt[x] - previous) + v;
|
|
}
|
|
else {
|
|
// use unfiltered value
|
|
s_anaFilt[x] = v * JITTER_ALPHA;
|
|
}
|
|
|
|
#if defined(JITTER_MEASURE)
|
|
if (JITTER_MEASURE_ACTIVE()) {
|
|
avgJitter[x].measure(ANA_FILT(x));
|
|
}
|
|
#endif
|
|
|
|
#define ANAFILT_MAX (2 * RESX * JITTER_ALPHA * ANALOG_MULTIPLIER - 1)
|
|
StepsCalibData * calib = (StepsCalibData *) &g_eeGeneral.calib[x];
|
|
if (IS_POT_MULTIPOS(x) && IS_MULTIPOS_CALIBRATED(calib)) {
|
|
// TODO: consider adding another low pass filter to eliminate multipos switching glitches
|
|
uint8_t vShifted = ANA_FILT(x) >> 4;
|
|
s_anaFilt[x] = ANAFILT_MAX;
|
|
for (uint32_t i=0; i<calib->count; i++) {
|
|
if (vShifted < calib->steps[i]) {
|
|
s_anaFilt[x] = (i * ANAFILT_MAX) / calib->count;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
#endif // #if !defined(SIMU)
|