591 lines
16 KiB
C++
591 lines
16 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"
|
|
#if defined(FLYSKY_GIMBAL)
|
|
#include "flysky_gimbal_driver.h"
|
|
#endif
|
|
|
|
#include "edgetx.h"
|
|
|
|
const etx_hal_adc_driver_t* _hal_adc_driver = nullptr;
|
|
const etx_hal_adc_inputs_t* _hal_adc_inputs = nullptr;
|
|
|
|
static uint16_t adcValues[MAX_ANALOG_INPUTS] __DMA_NO_CACHE;
|
|
|
|
bool adcInit(const etx_hal_adc_driver_t* driver)
|
|
{
|
|
// Init buffer, provides non random values before mixer task starts
|
|
memset(adcValues, 0, sizeof(adcValues));
|
|
|
|
// If there is an init function, it MUST succeed
|
|
if (driver && (!driver->init || driver->init())) {
|
|
_hal_adc_driver = driver;
|
|
_hal_adc_inputs = driver->inputs;
|
|
return true;
|
|
}
|
|
|
|
_hal_adc_driver = nullptr;
|
|
return false;
|
|
}
|
|
|
|
static bool adcSingleRead()
|
|
{
|
|
if (!_hal_adc_driver)
|
|
return false;
|
|
|
|
if (_hal_adc_driver->start_conversion &&
|
|
!_hal_adc_driver->start_conversion())
|
|
return false;
|
|
|
|
if (_hal_adc_driver->wait_completion)
|
|
_hal_adc_driver->wait_completion();
|
|
|
|
// Need to put all in a group to ensure DMA transfer will not affect ADC sampling
|
|
#if defined(FLYSKY_GIMBAL) && !defined(SIMU)
|
|
flysky_gimbal_start_read();
|
|
flysky_gimbal_wait_completion();
|
|
#endif
|
|
|
|
return true;
|
|
}
|
|
|
|
bool adcRead()
|
|
{
|
|
adcSingleRead();
|
|
|
|
// TODO: this hack needs to go away...
|
|
if (isVBatBridgeEnabled()) {
|
|
disableVBatBridge();
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
void adcCalibDefaults()
|
|
{
|
|
for (int i = 0; i < adcGetMaxCalibratedInputs(); i++) {
|
|
CalibData* calib = &g_eeGeneral.calib[i];
|
|
calib->mid = 1023;
|
|
calib->spanNeg = 1024 - (1024 / STICK_TOLERANCE);
|
|
calib->spanPos = 1024 - (1024 / STICK_TOLERANCE);
|
|
}
|
|
}
|
|
|
|
void adcCalibSetMidPoint()
|
|
{
|
|
uint8_t max_inputs = adcGetMaxCalibratedInputs();
|
|
uint8_t pot_offset = adcGetInputOffset(ADC_INPUT_FLEX);
|
|
|
|
for (uint8_t i = 0; i < max_inputs; i++) {
|
|
|
|
auto& calib = reusableBuffer.calib.inputs[i];
|
|
if (i < pot_offset || !IS_POT_MULTIPOS(i - pot_offset)) {
|
|
calib.input.loVal = 15000;
|
|
calib.input.hiVal = -15000;
|
|
calib.input.midVal = getAnalogValue(i) >> 1;
|
|
} else {
|
|
calib.xpot.stepsCount = 0;
|
|
calib.xpot.lastCount = 0;
|
|
memclear(calib.xpot.steps, sizeof(calib.xpot.steps));
|
|
}
|
|
}
|
|
}
|
|
|
|
#define XPOT_DELTA 10
|
|
#define XPOT_DELAY 10 /* cycles */
|
|
|
|
#define XPOT_CALIB_SHIFT 5
|
|
|
|
static void writeAnalogCalib(uint8_t input, int16_t low, int16_t mid, int16_t high)
|
|
{
|
|
auto& calib = g_eeGeneral.calib[input];
|
|
calib.mid = mid;
|
|
|
|
int16_t v = mid - low;
|
|
calib.spanNeg = v - v / STICK_TOLERANCE;
|
|
|
|
v = high - mid;
|
|
calib.spanPos = v - v / STICK_TOLERANCE;
|
|
}
|
|
|
|
static void writeXPotCalib(uint8_t input, int16_t* steps, uint8_t n_steps)
|
|
{
|
|
if (n_steps < 1) return;
|
|
|
|
StepsCalibData* calib = (StepsCalibData*)&g_eeGeneral.calib[input];
|
|
calib->count = n_steps - 1;
|
|
|
|
for (int i = 0; i < calib->count; i++) {
|
|
calib->steps[i] = (steps[i + 1] + steps[i]) >> XPOT_CALIB_SHIFT;
|
|
}
|
|
}
|
|
|
|
void adcCalibSetMinMax()
|
|
{
|
|
// get low and high vals for sticks and pots
|
|
uint8_t max_input = adcGetMaxCalibratedInputs();
|
|
uint8_t pot_offset = adcGetInputOffset(ADC_INPUT_FLEX);
|
|
|
|
for (uint8_t i = 0; i < max_input; i++) {
|
|
|
|
auto& calib = reusableBuffer.calib.inputs[i];
|
|
int16_t vt = getAnalogValue(i) >> 1;
|
|
|
|
if (i < pot_offset || !IS_POT_MULTIPOS(i - pot_offset)) {
|
|
calib.input.loVal = min(vt, calib.input.loVal);
|
|
calib.input.hiVal = max(vt, calib.input.hiVal);
|
|
|
|
if (i >= pot_offset) {
|
|
auto pot_cfg = POT_CONFIG(i - pot_offset);
|
|
if (pot_cfg == FLEX_POT || pot_cfg == FLEX_SWITCH || pot_cfg == FLEX_NONE) {
|
|
calib.input.midVal = (calib.input.hiVal + calib.input.loVal) / 2;
|
|
}
|
|
}
|
|
|
|
// in case we enough input movement, store the result
|
|
if (abs(calib.input.loVal - calib.input.hiVal) > 50) {
|
|
writeAnalogCalib(i, calib.input.loVal, calib.input.midVal, calib.input.hiVal);
|
|
}
|
|
} else {
|
|
auto& xpot = calib.xpot;
|
|
int count = xpot.stepsCount;
|
|
if (count <= XPOTS_MULTIPOS_COUNT) {
|
|
if (xpot.lastCount == 0 || vt < xpot.lastPosition - XPOT_DELTA ||
|
|
vt > xpot.lastPosition + XPOT_DELTA) {
|
|
xpot.lastPosition = vt;
|
|
xpot.lastCount = 1;
|
|
} else {
|
|
if (xpot.lastCount < 255) xpot.lastCount++;
|
|
}
|
|
if (xpot.lastCount == XPOT_DELAY) {
|
|
int16_t position = xpot.lastPosition;
|
|
bool found = false;
|
|
for (int j = 0; j < count; j++) {
|
|
int16_t step = xpot.steps[j];
|
|
if (position >= step - XPOT_DELTA &&
|
|
position <= step + XPOT_DELTA) {
|
|
found = true;
|
|
break;
|
|
}
|
|
}
|
|
if (!found) {
|
|
if (count < XPOTS_MULTIPOS_COUNT) {
|
|
int j = 0;
|
|
for (; j < count; j++) {
|
|
if (position < xpot.steps[j]) {
|
|
memmove(&xpot.steps[j + 1], &xpot.steps[j],
|
|
(count - j) * sizeof(int16_t));
|
|
break;
|
|
}
|
|
}
|
|
xpot.steps[j] = position;
|
|
}
|
|
|
|
xpot.stepsCount = ++count;
|
|
writeXPotCalib(i, xpot.steps, count);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
static void disableUncalibratedXPots()
|
|
{
|
|
uint8_t pot_offset = adcGetInputOffset(ADC_INPUT_FLEX);
|
|
uint8_t max_pots = adcGetMaxInputs(ADC_INPUT_FLEX);
|
|
|
|
for (uint8_t i = 0; i < max_pots; i++) {
|
|
if (IS_POT_MULTIPOS(i)) {
|
|
StepsCalibData* calib = (StepsCalibData*)&g_eeGeneral.calib[i + pot_offset];
|
|
if(!IS_MULTIPOS_CALIBRATED(calib)) {
|
|
// not enough config points
|
|
#if defined(XPOS_CALIB_DEFAULT)
|
|
size_t index = 0;
|
|
calib->count = XPOTS_MULTIPOS_COUNT - 1;
|
|
for (const auto &value : XPOS_CALIB_DEFAULT) calib->steps[index++] = value;
|
|
#else
|
|
g_eeGeneral.potsConfig &= POT_CONFIG_DISABLE_MASK(i);
|
|
#endif
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void adcCalibStore()
|
|
{
|
|
disableUncalibratedXPots();
|
|
g_eeGeneral.chkSum = evalChkSum();
|
|
storageDirty(EE_GENERAL);
|
|
}
|
|
|
|
uint16_t getAnalogValue(uint8_t index)
|
|
{
|
|
if (index >= MAX_ANALOG_INPUTS) return 0;
|
|
return adcValues[index];
|
|
}
|
|
|
|
void setAnalogValue(uint8_t index, uint16_t value)
|
|
{
|
|
if (index >= MAX_ANALOG_INPUTS) return;
|
|
adcValues[index] = value;
|
|
}
|
|
|
|
uint16_t* getAnalogValues()
|
|
{
|
|
return adcValues;
|
|
}
|
|
|
|
// used by diaganas
|
|
uint32_t s_anaFilt[MAX_ANALOG_INPUTS];
|
|
|
|
#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)
|
|
{
|
|
if (chan >= MAX_ANALOG_INPUTS) return 0;
|
|
return ANA_FILT(chan);
|
|
}
|
|
|
|
uint32_t anaIn_diag(uint8_t chan)
|
|
{
|
|
if (chan >= MAX_ANALOG_INPUTS) return 0;
|
|
return s_anaFilt[chan] / JITTER_ALPHA;
|
|
}
|
|
|
|
void anaSetFiltered(uint8_t chan, uint16_t val)
|
|
{
|
|
val += RESX;
|
|
s_anaFilt[chan] = val * (JITTER_ALPHA * ANALOG_MULTIPLIER);
|
|
}
|
|
|
|
void anaResetFiltered()
|
|
{
|
|
memset(s_anaFilt, 0, sizeof(s_anaFilt));
|
|
}
|
|
|
|
#if defined(JITTER_MEASURE)
|
|
JitterMeter<uint16_t> rawJitter[MAX_ANALOG_INPUTS];
|
|
JitterMeter<uint16_t> avgJitter[MAX_ANALOG_INPUTS];
|
|
tmr10ms_t jitterResetTime = 0;
|
|
#endif
|
|
|
|
static uint32_t apply_low_pass_filter(uint32_t v, uint32_t v_prev,
|
|
bool is_main_input)
|
|
{
|
|
// 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
|
|
//
|
|
uint32_t previous = v_prev / 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
|
|
// Please note: these settings only apply to main controls.
|
|
bool useJitterFilter = true;
|
|
if (is_main_input) {
|
|
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);
|
|
}
|
|
}
|
|
|
|
uint32_t out;
|
|
if (useJitterFilter && diff < (10 * ANALOG_MULTIPLIER)) {
|
|
// apply jitter filter
|
|
out = (v_prev - previous) + v;
|
|
} else {
|
|
// use unfiltered value
|
|
out = v * JITTER_ALPHA;
|
|
}
|
|
|
|
return out;
|
|
}
|
|
|
|
static uint32_t apply_calibration(const CalibData* calib, uint32_t v)
|
|
{
|
|
// Apply calibration relative to mid-point
|
|
int32_t s = v - 2 * calib->mid;
|
|
s = s * (int32_t)RESX /
|
|
(max((int16_t)100, (s > 0 ? calib->spanPos : calib->spanNeg)));
|
|
|
|
// Translate back in range
|
|
s += 2 * RESX;
|
|
|
|
// Limit values to supported range
|
|
if (s < 0) {
|
|
s = 0;
|
|
} else if (s > 4 * RESX) {
|
|
s = 4 * RESX;
|
|
}
|
|
|
|
return s;
|
|
}
|
|
|
|
static uint32_t apply_multipos(const StepsCalibData* calib, uint32_t v)
|
|
{
|
|
constexpr uint32_t ALPHA_MULT = JITTER_ALPHA * ANALOG_MULTIPLIER;
|
|
constexpr uint32_t ANAFILT_MAX = 2 * RESX * ALPHA_MULT;
|
|
|
|
// TODO: consider adding another low pass filter to eliminate multipos
|
|
// switching glitches
|
|
uint8_t vShifted = (v / ALPHA_MULT) >> 4;
|
|
|
|
for (uint32_t i = 0; i < calib->count; i++) {
|
|
if (vShifted < calib->steps[i]) {
|
|
return (i * (ANAFILT_MAX + ALPHA_MULT)) / calib->count;
|
|
}
|
|
}
|
|
|
|
return ANAFILT_MAX;
|
|
}
|
|
|
|
#if defined(SIMU)
|
|
// In simulation, assume equally spaced positions across full ADC range
|
|
static uint32_t apply_multipos_simu(uint32_t v)
|
|
{
|
|
constexpr uint32_t ALPHA_MULT = JITTER_ALPHA * ANALOG_MULTIPLIER;
|
|
constexpr uint32_t ANAFILT_MAX = 2 * RESX * ALPHA_MULT;
|
|
constexpr uint32_t N = XPOTS_MULTIPOS_COUNT;
|
|
constexpr uint32_t COUNT = N - 1;
|
|
|
|
uint32_t pos = v * N / (ANAFILT_MAX + 1);
|
|
if (pos > COUNT) pos = COUNT;
|
|
|
|
return (pos < COUNT)
|
|
? (pos * (ANAFILT_MAX + ALPHA_MULT)) / COUNT
|
|
: ANAFILT_MAX;
|
|
}
|
|
#endif
|
|
|
|
void getADC()
|
|
{
|
|
auto max_analogs = adcGetMaxInputs(ADC_INPUT_ALL);
|
|
auto max_mains = adcGetMaxInputs(ADC_INPUT_MAIN);
|
|
auto max_pots = adcGetMaxInputs(ADC_INPUT_FLEX);
|
|
auto pot_offset = adcGetInputOffset(ADC_INPUT_FLEX);
|
|
auto max_calib_analogs = adcGetMaxCalibratedInputs();
|
|
|
|
#if defined(JITTER_MEASURE)
|
|
if (JITTER_MEASURE_ACTIVE() && jitterResetTime < get_tmr10ms()) {
|
|
// reset jitter measurement every second
|
|
for (uint32_t x = 0; x < max_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 < max_analogs; x++) {
|
|
|
|
bool is_flex_input = (x >= pot_offset) && (x < pot_offset + max_pots);
|
|
bool is_multipos = is_flex_input && IS_POT_MULTIPOS(x - pot_offset);
|
|
|
|
uint32_t v = getAnalogValue(x);
|
|
|
|
#if !defined(SIMU)
|
|
// Apply hardware calibration (not needed in simulation:
|
|
// the host already provides normalized ADC-range values)
|
|
if (x < max_calib_analogs && !is_multipos) {
|
|
v = apply_calibration(&g_eeGeneral.calib[x], v);
|
|
}
|
|
#endif
|
|
|
|
// Apply inversion
|
|
if (x < pot_offset && getStickInversion(inputMappingConvertMode(x))) {
|
|
v = 4 * RESX - v;
|
|
}
|
|
|
|
if (is_flex_input && getPotInversion(x - pot_offset)) {
|
|
v = 4 * RESX - v;
|
|
}
|
|
|
|
// Apply filtering
|
|
s_anaFilt[x] = apply_low_pass_filter(v, s_anaFilt[x], x < max_mains);
|
|
|
|
if (is_multipos) {
|
|
#if defined(SIMU)
|
|
s_anaFilt[x] = apply_multipos_simu(s_anaFilt[x]);
|
|
#else
|
|
const auto* calib = (const StepsCalibData*)&g_eeGeneral.calib[x];
|
|
if (IS_MULTIPOS_CALIBRATED(calib)) {
|
|
s_anaFilt[x] = apply_multipos(calib, s_anaFilt[x]);
|
|
}
|
|
#endif
|
|
}
|
|
|
|
#if defined(JITTER_MEASURE)
|
|
if (JITTER_MEASURE_ACTIVE()) {
|
|
avgJitter[x].measure(ANA_FILT(x));
|
|
}
|
|
#endif
|
|
}
|
|
}
|
|
|
|
potconfig_t adcGetDefaultPotsConfig()
|
|
{
|
|
if (!_hal_adc_driver) return 0;
|
|
return _hal_adc_driver->default_pots_cfg;
|
|
}
|
|
|
|
uint8_t adcGetMaxInputs(uint8_t type)
|
|
{
|
|
if (type > ADC_INPUT_ALL) return 0;
|
|
return _hal_adc_inputs[type].n_inputs;
|
|
}
|
|
|
|
uint8_t adcGetInputOffset(uint8_t type)
|
|
{
|
|
if (type > ADC_INPUT_ALL) return 0;
|
|
return _hal_adc_inputs[type].offset;
|
|
}
|
|
|
|
uint8_t adcGetMaxCalibratedInputs()
|
|
{
|
|
// ADC_INPUT_MAIN + ADC_INPUT_FLEX + ADC_INPUT_AXIS
|
|
return adcGetInputOffset(ADC_INPUT_VBAT);
|
|
}
|
|
|
|
uint16_t adcGetInputValue(uint8_t type, uint8_t idx)
|
|
{
|
|
if (type >= ADC_INPUT_ALL) return 0;
|
|
|
|
const auto& inputs = _hal_adc_inputs[type];
|
|
auto n_inputs = inputs.n_inputs;
|
|
auto offset = inputs.offset;
|
|
if (!n_inputs || idx >= n_inputs) return 0;
|
|
|
|
return getAnalogValue(offset + idx);
|
|
}
|
|
|
|
const char* adcGetInputName(uint8_t type, uint8_t idx)
|
|
{
|
|
if (type >= ADC_INPUT_ALL ||
|
|
idx >= _hal_adc_inputs[type].n_inputs)
|
|
return "";
|
|
|
|
return _hal_adc_inputs[type].inputs[idx].name;
|
|
}
|
|
|
|
const char* adcGetInputName(uint8_t idx)
|
|
{
|
|
uint8_t type = ADC_INPUT_MAIN;
|
|
|
|
// find the proper input type
|
|
while (_hal_adc_inputs[type].offset + _hal_adc_inputs[type].n_inputs <= idx) {
|
|
if (++type > ADC_INPUT_FLEX) return nullptr;
|
|
}
|
|
|
|
idx -= _hal_adc_inputs[type].offset;
|
|
return _hal_adc_inputs[type].inputs[idx].name;
|
|
}
|
|
|
|
int adcGetInputIdx(const char* input, uint8_t len)
|
|
{
|
|
int idx = 0;
|
|
uint8_t type = ADC_INPUT_MAIN;
|
|
|
|
do {
|
|
for (uint8_t i = 0; i < _hal_adc_inputs[type].n_inputs; i++, idx++) {
|
|
if (!strncmp(_hal_adc_inputs[type].inputs[i].name, input, len))
|
|
return idx;
|
|
}
|
|
} while (++type < ADC_INPUT_ALL);
|
|
|
|
return -1;
|
|
}
|
|
|
|
const char* adcGetInputLabel(uint8_t type, uint8_t idx)
|
|
{
|
|
if (type >= ADC_INPUT_ALL ||
|
|
idx >= _hal_adc_inputs[type].n_inputs)
|
|
return "";
|
|
|
|
return _hal_adc_inputs[type].inputs[idx].label;
|
|
}
|
|
|
|
const char* adcGetInputShortLabel(uint8_t type, uint8_t idx)
|
|
{
|
|
if (type >= ADC_INPUT_ALL ||
|
|
idx >= _hal_adc_inputs[type].n_inputs)
|
|
return "";
|
|
|
|
return _hal_adc_inputs[type].inputs[idx].short_label;
|
|
}
|
|
|
|
void adcSetInputMask(uint32_t mask)
|
|
{
|
|
if (_hal_adc_driver && _hal_adc_driver->set_input_mask) {
|
|
_hal_adc_driver->set_input_mask(mask);
|
|
}
|
|
}
|
|
|
|
uint32_t adcGetInputMask()
|
|
{
|
|
return _hal_adc_driver && _hal_adc_driver->get_input_mask
|
|
? _hal_adc_driver->get_input_mask()
|
|
: 0;
|
|
}
|