From b5214f900b55fb0811b5b0d02cd0b172c3f0d2e9 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sat, 13 Nov 2021 17:05:21 +0100 Subject: [PATCH 01/17] Allow an ability to track multiple peak in the gyro signal --- src/main/flight/dynamic_gyro_notch.c | 46 +++++++---- src/main/flight/dynamic_gyro_notch.h | 13 ++- src/main/flight/gyroanalyse.c | 115 ++++++++++++++++++--------- src/main/flight/gyroanalyse.h | 16 +++- src/main/sensors/gyro.c | 4 +- src/main/sensors/gyro.h | 5 ++ 6 files changed, 131 insertions(+), 68 deletions(-) diff --git a/src/main/flight/dynamic_gyro_notch.c b/src/main/flight/dynamic_gyro_notch.c index 7e6c12afbc..fa4c48e913 100644 --- a/src/main/flight/dynamic_gyro_notch.c +++ b/src/main/flight/dynamic_gyro_notch.c @@ -26,6 +26,8 @@ #ifdef USE_DYNAMIC_FILTERS +FILE_COMPILE_FOR_SPEED + #include #include "dynamic_gyro_notch.h" #include "fc/config.h" @@ -33,7 +35,12 @@ #include "sensors/gyro.h" void dynamicGyroNotchFiltersInit(dynamicGyroNotchState_t *state) { - state->filtersApplyFn = nullFilterApply; + + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) { + state->filtersApplyFn[axis][i] = nullFilterApply; + } + } state->dynNotchQ = gyroConfig()->dynamicGyroNotchQ / 100.0f; state->enabled = gyroConfig()->dynamicGyroNotchEnabled; @@ -45,27 +52,32 @@ void dynamicGyroNotchFiltersInit(dynamicGyroNotchState_t *state) { */ for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { //Any initial notch Q is valid sice it will be updated immediately after - biquadFilterInit(&state->filters[axis][0], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH); - biquadFilterInit(&state->filters[axis][1], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH); - biquadFilterInit(&state->filters[axis][2], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH); + for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) { + biquadFilterInit(&state->filters[axis][i], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH); + state->filtersApplyFn[axis][i] = (filterApplyFnPtr)biquadFilterApplyDF1; + } + } - state->filtersApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; } } -void dynamicGyroNotchFiltersUpdate(dynamicGyroNotchState_t *state, int axis, uint16_t frequency) { +void dynamicGyroNotchFiltersUpdate(dynamicGyroNotchState_t *state, int axis, float frequency[]) { - state->frequency[axis] = frequency; - - DEBUG_SET(DEBUG_DYNAMIC_FILTER_FREQUENCY, axis, frequency); - - if (state->enabled) { - biquadFilterUpdate(&state->filters[0][axis], frequency, state->looptime, state->dynNotchQ, FILTER_NOTCH); - biquadFilterUpdate(&state->filters[1][axis], frequency, state->looptime, state->dynNotchQ, FILTER_NOTCH); - biquadFilterUpdate(&state->filters[2][axis], frequency, state->looptime, state->dynNotchQ, FILTER_NOTCH); + if (axis == FD_ROLL) { + for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) { + DEBUG_SET(DEBUG_DYNAMIC_FILTER_FREQUENCY, i, frequency[i]); + } } + if (state->enabled) { + for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) { + // Filter update happens only if peak was detected + if (frequency[i] > 0.0f) { + biquadFilterUpdate(&state->filters[axis][i], frequency[i], state->looptime, state->dynNotchQ, FILTER_NOTCH); + } + } + } } float dynamicGyroNotchFiltersApply(dynamicGyroNotchState_t *state, int axis, float input) { @@ -75,9 +87,9 @@ float dynamicGyroNotchFiltersApply(dynamicGyroNotchState_t *state, int axis, flo * We always apply all filters. If a filter dimension is disabled, one of * the function pointers will be a null apply function */ - output = state->filtersApplyFn((filter_t *)&state->filters[axis][0], output); - output = state->filtersApplyFn((filter_t *)&state->filters[axis][1], output); - output = state->filtersApplyFn((filter_t *)&state->filters[axis][2], output); + for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) { + output = state->filtersApplyFn[axis][i]((filter_t *)&state->filters[axis][i], output); + } return output; } diff --git a/src/main/flight/dynamic_gyro_notch.h b/src/main/flight/dynamic_gyro_notch.h index 87d923fb06..c5be6a8137 100644 --- a/src/main/flight/dynamic_gyro_notch.h +++ b/src/main/flight/dynamic_gyro_notch.h @@ -27,23 +27,22 @@ #include #include "common/axis.h" #include "common/filter.h" +#include "sensors/gyro.h" #define DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 350 typedef struct dynamicGyroNotchState_s { - uint16_t frequency[XYZ_AXIS_COUNT]; + // uint16_t frequency[XYZ_AXIS_COUNT]; float dynNotchQ; float dynNotch1Ctr; float dynNotch2Ctr; uint32_t looptime; uint8_t enabled; - /* - * Dynamic gyro filter can be 3x1, 3x2 or 3x3 depending on filter type - */ - biquadFilter_t filters[XYZ_AXIS_COUNT][XYZ_AXIS_COUNT]; - filterApplyFnPtr filtersApplyFn; + + biquadFilter_t filters[XYZ_AXIS_COUNT][DYN_NOTCH_PEAK_COUNT]; + filterApplyFnPtr filtersApplyFn[XYZ_AXIS_COUNT][DYN_NOTCH_PEAK_COUNT]; } dynamicGyroNotchState_t; void dynamicGyroNotchFiltersInit(dynamicGyroNotchState_t *state); -void dynamicGyroNotchFiltersUpdate(dynamicGyroNotchState_t *state, int axis, uint16_t frequency); +void dynamicGyroNotchFiltersUpdate(dynamicGyroNotchState_t *state, int axis, float frequency[]); float dynamicGyroNotchFiltersApply(dynamicGyroNotchState_t *state, int axis, float input); diff --git a/src/main/flight/gyroanalyse.c b/src/main/flight/gyroanalyse.c index 2037b4712c..fd08f6d61e 100644 --- a/src/main/flight/gyroanalyse.c +++ b/src/main/flight/gyroanalyse.c @@ -61,7 +61,7 @@ enum { // NB FFT_WINDOW_SIZE is set to 32 in gyroanalyse.h #define FFT_BIN_COUNT (FFT_WINDOW_SIZE / 2) // smoothing frequency for FFT centre frequency -#define DYN_NOTCH_SMOOTH_FREQ_HZ 50 +#define DYN_NOTCH_SMOOTH_FREQ_HZ 35 /* * Slow down gyro sample acquisition. This lowers the max frequency but increases the resolution. @@ -93,11 +93,12 @@ void gyroDataAnalyseStateInit( const uint32_t filterUpdateUs = targetLooptimeUs * STEP_COUNT * XYZ_AXIS_COUNT; for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - // any init value - state->centerFreq[axis] = state->maxFrequency; - state->prevCenterFreq[axis] = state->maxFrequency; + + for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) { + state->centerFrequency[axis][i] = state->maxFrequency; + pt2FilterInit(&state->detectedFrequencyFilter[axis][i], pt2FilterGain(DYN_NOTCH_SMOOTH_FREQ_HZ, filterUpdateUs * 1e-6f)); + } - biquadFilterInitLPF(&state->detectedFrequencyFilter[axis], DYN_NOTCH_SMOOTH_FREQ_HZ, filterUpdateUs); } } @@ -135,18 +136,6 @@ void stage_rfft_f32(arm_rfft_fast_instance_f32 *S, float32_t *p, float32_t *pOut void arm_cfft_radix8by4_f32(arm_cfft_instance_f32 *S, float32_t *p1); void arm_bitreversal_32(uint32_t *pSrc, const uint16_t bitRevLen, const uint16_t *pBitRevTable); -static uint8_t findPeakBinIndex(gyroAnalyseState_t *state) { - uint8_t peakBinIndex = state->fftStartBin; - float peakValue = 0; - for (int i = state->fftStartBin; i < FFT_BIN_COUNT; i++) { - if (state->fftData[i] > peakValue) { - peakValue = state->fftData[i]; - peakBinIndex = i; - } - } - return peakBinIndex; -} - static float computeParabolaMean(gyroAnalyseState_t *state, uint8_t peakBinIndex) { float preciseBin = peakBinIndex; @@ -190,38 +179,86 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state) // 8us arm_cmplx_mag_f32(state->rfftData, state->fftData, FFT_BIN_COUNT); - //Find peak frequency - uint8_t peakBin = findPeakBinIndex(state); + //Compute mean - needed to work with only peaks above the noise floor + state->fftMeanValue = 0; + for (int bin = (state->fftStartBin + 1); bin < FFT_BIN_COUNT; bin++) { + state->fftMeanValue += state->fftData[bin]; + } + state->fftMeanValue /= FFT_BIN_COUNT - state->fftStartBin - 1; - // Failsafe to ensure the last bin is not a peak bin - peakBin = constrain(peakBin, state->fftStartBin, FFT_BIN_COUNT - 1); + //Zero the data structure + for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) { + state->peaks[i].bin = 0; + state->peaks[i].value = 0.0f; + } - /* - * Calculate center frequency using the parabola method - */ - float preciseBin = computeParabolaMean(state, peakBin); - float peakFrequency = preciseBin * state->fftResolution; + // Find peaks + for (int bin = (state->fftStartBin + 1); bin < FFT_BIN_COUNT - 1; bin++) { + /* + * Peak is defined if the current bin is greater than the previous bin and the next bin + */ + if ( + state->fftData[bin] > state->fftMeanValue && + state->fftData[bin] > state->fftData[bin - 1] && + state->fftData[bin] > state->fftData[bin + 1] + ) { + /* + * We are only interested in N biggest peaks + * Check previously found peaks and update the structure if necessary + */ + for (int p = 0; p < DYN_NOTCH_PEAK_COUNT; p++) { + if (state->fftData[bin] > state->peaks[p].value) { + for (int k = DYN_NOTCH_PEAK_COUNT - 1; k > p; k--) { + state->peaks[k] = state->peaks[k - 1]; + } + state->peaks[p].bin = bin; + state->peaks[p].value = state->fftData[bin]; + break; + } + } + bin++; // If bin is peak, next bin can't be peak => jump it + } + } - peakFrequency = biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], peakFrequency); - peakFrequency = constrainf(peakFrequency, state->minFrequency, state->maxFrequency); + // Sort N biggest peaks in ascending bin order (example: 3, 8, 25, 0, 0, ..., 0) + for (int p = DYN_NOTCH_PEAK_COUNT - 1; p > 0; p--) { + for (int k = 0; k < p; k++) { + // Swap peaks but ignore swapping void peaks (bin = 0). This leaves + // void peaks at the end of peaks array without moving them + if (state->peaks[k].bin > state->peaks[k + 1].bin && state->peaks[k + 1].bin != 0) { + peak_t temp = state->peaks[k]; + state->peaks[k] = state->peaks[k + 1]; + state->peaks[k + 1] = temp; + } + } + } - state->prevCenterFreq[state->updateAxis] = state->centerFreq[state->updateAxis]; - state->centerFreq[state->updateAxis] = peakFrequency; break; } case STEP_UPDATE_FILTERS_AND_HANNING: { - // 7us - // calculate cutoffFreq and notch Q, update notch filter =1.8+((A2-150)*0.004) - if (state->prevCenterFreq[state->updateAxis] != state->centerFreq[state->updateAxis]) { - /* - * Filters will be updated inside dynamicGyroNotchFiltersUpdate() - */ - state->filterUpdateExecute = true; - state->filterUpdateAxis = state->updateAxis; - state->filterUpdateFrequency = state->centerFreq[state->updateAxis]; + + /* + * Update frequencies + */ + for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) { + + if (state->peaks[i].bin > 0) { + const int bin = constrain(state->peaks[i].bin, state->fftStartBin, FFT_BIN_COUNT - 1); + float frequency = computeParabolaMean(state, bin) * state->fftResolution; + + state->centerFrequency[state->updateAxis][i] = pt2FilterApply(&state->detectedFrequencyFilter[state->updateAxis][i], frequency); + } else { + state->centerFrequency[state->updateAxis][i] = 0.0f; + } } + /* + * Filters will be updated inside dynamicGyroNotchFiltersUpdate() + */ + state->filterUpdateExecute = true; + state->filterUpdateAxis = state->updateAxis; + //Switch to the next axis state->updateAxis = (state->updateAxis + 1) % XYZ_AXIS_COUNT; diff --git a/src/main/flight/gyroanalyse.h b/src/main/flight/gyroanalyse.h index 0d95be42cc..01b9a3c608 100644 --- a/src/main/flight/gyroanalyse.h +++ b/src/main/flight/gyroanalyse.h @@ -31,6 +31,11 @@ */ #define FFT_WINDOW_SIZE 64 +typedef struct peak_s { + int bin; + float value; +} peak_t; + typedef struct gyroAnalyseState_s { // accumulator for oversampled data => no aliasing and less noise float currentSample[XYZ_AXIS_COUNT]; @@ -48,12 +53,17 @@ typedef struct gyroAnalyseState_s { float fftData[FFT_WINDOW_SIZE]; float rfftData[FFT_WINDOW_SIZE]; - biquadFilter_t detectedFrequencyFilter[XYZ_AXIS_COUNT]; - uint16_t centerFreq[XYZ_AXIS_COUNT]; - uint16_t prevCenterFreq[XYZ_AXIS_COUNT]; + float fftMeanValue; + + pt2Filter_t detectedFrequencyFilter[XYZ_AXIS_COUNT][DYN_NOTCH_PEAK_COUNT]; + float centerFrequency[XYZ_AXIS_COUNT][DYN_NOTCH_PEAK_COUNT]; + + peak_t peaks[DYN_NOTCH_PEAK_COUNT]; + bool filterUpdateExecute; uint8_t filterUpdateAxis; uint16_t filterUpdateFrequency; + uint16_t fftSamplingRateHz; uint8_t fftStartBin; float fftResolution; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 8b14932ce2..d5ba164ef2 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -479,8 +479,8 @@ void FAST_CODE NOINLINE gyroFilter() if (gyroAnalyseState.filterUpdateExecute) { dynamicGyroNotchFiltersUpdate( &dynamicGyroNotchState, - gyroAnalyseState.filterUpdateAxis, - gyroAnalyseState.filterUpdateFrequency + gyroAnalyseState.filterUpdateAxis, + gyroAnalyseState.centerFrequency[gyroAnalyseState.filterUpdateAxis] ); } } diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 44d567e570..18a3fe4ecb 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -24,6 +24,11 @@ #include "config/parameter_group.h" #include "drivers/sensor.h" +/* + * Number of peaks to detect with Dynamic Notch Filter aka Matrixc Filter. This is equal to the number of dynamic notch filters + */ +#define DYN_NOTCH_PEAK_COUNT 3 + typedef enum { GYRO_NONE = 0, GYRO_AUTODETECT, From e5122f56512cfa5b804e125358d7951e05aff162 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sat, 13 Nov 2021 17:16:17 +0100 Subject: [PATCH 02/17] reverrt to PT1 filter --- src/main/flight/gyroanalyse.c | 4 ++-- src/main/flight/gyroanalyse.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/flight/gyroanalyse.c b/src/main/flight/gyroanalyse.c index fd08f6d61e..48ac714466 100644 --- a/src/main/flight/gyroanalyse.c +++ b/src/main/flight/gyroanalyse.c @@ -96,7 +96,7 @@ void gyroDataAnalyseStateInit( for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) { state->centerFrequency[axis][i] = state->maxFrequency; - pt2FilterInit(&state->detectedFrequencyFilter[axis][i], pt2FilterGain(DYN_NOTCH_SMOOTH_FREQ_HZ, filterUpdateUs * 1e-6f)); + pt1FilterInit(&state->detectedFrequencyFilter[axis][i], DYN_NOTCH_SMOOTH_FREQ_HZ, filterUpdateUs * 1e-6f); } } @@ -247,7 +247,7 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state) const int bin = constrain(state->peaks[i].bin, state->fftStartBin, FFT_BIN_COUNT - 1); float frequency = computeParabolaMean(state, bin) * state->fftResolution; - state->centerFrequency[state->updateAxis][i] = pt2FilterApply(&state->detectedFrequencyFilter[state->updateAxis][i], frequency); + state->centerFrequency[state->updateAxis][i] = pt1FilterApply(&state->detectedFrequencyFilter[state->updateAxis][i], frequency); } else { state->centerFrequency[state->updateAxis][i] = 0.0f; } diff --git a/src/main/flight/gyroanalyse.h b/src/main/flight/gyroanalyse.h index 01b9a3c608..d4ab2663f6 100644 --- a/src/main/flight/gyroanalyse.h +++ b/src/main/flight/gyroanalyse.h @@ -55,7 +55,7 @@ typedef struct gyroAnalyseState_s { float fftMeanValue; - pt2Filter_t detectedFrequencyFilter[XYZ_AXIS_COUNT][DYN_NOTCH_PEAK_COUNT]; + pt1Filter_t detectedFrequencyFilter[XYZ_AXIS_COUNT][DYN_NOTCH_PEAK_COUNT]; float centerFrequency[XYZ_AXIS_COUNT][DYN_NOTCH_PEAK_COUNT]; peak_t peaks[DYN_NOTCH_PEAK_COUNT]; From da4a6d4620e744c75ae2648be6eaa24fd70724df Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 14 Nov 2021 14:31:17 +0100 Subject: [PATCH 03/17] Lower tracking filter cutoff frequency --- src/main/flight/gyroanalyse.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/gyroanalyse.c b/src/main/flight/gyroanalyse.c index 48ac714466..a8a8fb6795 100644 --- a/src/main/flight/gyroanalyse.c +++ b/src/main/flight/gyroanalyse.c @@ -61,7 +61,7 @@ enum { // NB FFT_WINDOW_SIZE is set to 32 in gyroanalyse.h #define FFT_BIN_COUNT (FFT_WINDOW_SIZE / 2) // smoothing frequency for FFT centre frequency -#define DYN_NOTCH_SMOOTH_FREQ_HZ 35 +#define DYN_NOTCH_SMOOTH_FREQ_HZ 25 /* * Slow down gyro sample acquisition. This lowers the max frequency but increases the resolution. From d61538a3bfdc4c1d8033f7fea9aa834784a061d6 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 14 Nov 2021 17:15:21 +0100 Subject: [PATCH 04/17] Drop above mean condition for peaks --- src/main/flight/gyroanalyse.c | 8 -------- src/main/flight/gyroanalyse.h | 3 --- 2 files changed, 11 deletions(-) diff --git a/src/main/flight/gyroanalyse.c b/src/main/flight/gyroanalyse.c index a8a8fb6795..7596583273 100644 --- a/src/main/flight/gyroanalyse.c +++ b/src/main/flight/gyroanalyse.c @@ -179,13 +179,6 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state) // 8us arm_cmplx_mag_f32(state->rfftData, state->fftData, FFT_BIN_COUNT); - //Compute mean - needed to work with only peaks above the noise floor - state->fftMeanValue = 0; - for (int bin = (state->fftStartBin + 1); bin < FFT_BIN_COUNT; bin++) { - state->fftMeanValue += state->fftData[bin]; - } - state->fftMeanValue /= FFT_BIN_COUNT - state->fftStartBin - 1; - //Zero the data structure for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) { state->peaks[i].bin = 0; @@ -198,7 +191,6 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state) * Peak is defined if the current bin is greater than the previous bin and the next bin */ if ( - state->fftData[bin] > state->fftMeanValue && state->fftData[bin] > state->fftData[bin - 1] && state->fftData[bin] > state->fftData[bin + 1] ) { diff --git a/src/main/flight/gyroanalyse.h b/src/main/flight/gyroanalyse.h index d4ab2663f6..2cfd19389f 100644 --- a/src/main/flight/gyroanalyse.h +++ b/src/main/flight/gyroanalyse.h @@ -45,7 +45,6 @@ typedef struct gyroAnalyseState_s { float downsampledGyroData[XYZ_AXIS_COUNT][FFT_WINDOW_SIZE]; // update state machine step information - uint8_t updateTicks; uint8_t updateStep; uint8_t updateAxis; @@ -53,8 +52,6 @@ typedef struct gyroAnalyseState_s { float fftData[FFT_WINDOW_SIZE]; float rfftData[FFT_WINDOW_SIZE]; - float fftMeanValue; - pt1Filter_t detectedFrequencyFilter[XYZ_AXIS_COUNT][DYN_NOTCH_PEAK_COUNT]; float centerFrequency[XYZ_AXIS_COUNT][DYN_NOTCH_PEAK_COUNT]; From d40925895e72a1af8fedd0cbec52d42da635cd46 Mon Sep 17 00:00:00 2001 From: OptimusTi Date: Thu, 25 Nov 2021 20:03:31 -0500 Subject: [PATCH 05/17] Rework of RSSI% for CRSF New way to represent RSSI% from dBm. --- src/main/cms/cms_menu_osd.c | 1 + src/main/fc/settings.yaml | 16 ++++++++++++++++ src/main/io/osd.c | 2 ++ src/main/io/osd.h | 2 ++ src/main/rx/crsf.c | 12 ++++++++++-- 5 files changed, 31 insertions(+), 2 deletions(-) diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c index f95b7bef2a..1cff131e75 100644 --- a/src/main/cms/cms_menu_osd.c +++ b/src/main/cms/cms_menu_osd.c @@ -146,6 +146,7 @@ static const OSD_Entry menuCrsfRxEntries[]= OSD_SETTING_ENTRY("LQ FORMAT", SETTING_OSD_CRSF_LQ_FORMAT), OSD_SETTING_ENTRY("LQ ALARM LEVEL", SETTING_OSD_LINK_QUALITY_ALARM), OSD_SETTING_ENTRY("SNR ALARM LEVEL", SETTING_OSD_SNR_ALARM), + OSD_SETTING_ENTRY("RX SENSITIVITY", SETTING_OSD_RSSI_DBM_MIN), OSD_ELEMENT_ENTRY("RX RSSI DBM", OSD_CRSF_RSSI_DBM), OSD_ELEMENT_ENTRY("RX LQ", OSD_CRSF_LQ), OSD_ELEMENT_ENTRY("RX SNR ALARM", OSD_CRSF_SNR_DB), diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 22dd0335b1..e5018b0fb3 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3486,6 +3486,22 @@ groups: max: 6 default_value: 3 + - name: osd_rssi_dbm_max + condition: USE_SERIALRX_CRSF + description: "RSSI dBm upper end of curve. Perfect rssi (max) = 100%" + default_value: -30 + field: rssi_dbm_max + min: -50 + max: 0 + + - name: osd_rssi_dbm_min + condition: USE_SERIALRX_CRSF + description: "RSSI dBm lower end of curve or RX sensitivity level. Worst rssi (min) = 0%" + default_value: -120 + field: rssi_dbm_min + min: -130 + max: 0 + - name: PG_OSD_COMMON_CONFIG type: osdCommonConfig_t headers: ["io/osd_common.h"] diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 0c21c4c3ea..88c8e50382 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -3189,6 +3189,8 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .crsf_lq_format = SETTING_OSD_CRSF_LQ_FORMAT_DEFAULT, .link_quality_alarm = SETTING_OSD_LINK_QUALITY_ALARM_DEFAULT, .rssi_dbm_alarm = SETTING_OSD_RSSI_DBM_ALARM_DEFAULT, + .rssi_dbm_max = SETTING_OSD_RSSI_DBM_MAX_DEFAULT, + .rssi_dbm_min = SETTING_OSD_RSSI_DBM_MIN_DEFAULT, #endif #ifdef USE_TEMPERATURE_SENSOR .temp_label_align = SETTING_OSD_TEMP_LABEL_ALIGN_DEFAULT, diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 75b8aefeff..ded916981f 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -323,6 +323,8 @@ typedef struct osdConfig_s { int8_t snr_alarm; //CRSF SNR alarm in dB int8_t link_quality_alarm; int16_t rssi_dbm_alarm; // in dBm + int16_t rssi_dbm_max; // Perfect RSSI. Set to High end of curve. RSSI at 100% + int16_t rssi_dbm_min; // Worst RSSI. Set to low end of curve or RX sensitivity level. RSSI at 0% #endif #ifdef USE_BARO int16_t baro_temp_alarm_min; diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c index f01dcb965e..705abd9308 100755 --- a/src/main/rx/crsf.c +++ b/src/main/rx/crsf.c @@ -36,6 +36,7 @@ FILE_COMPILE_FOR_SPEED #include "drivers/serial_uart.h" #include "io/serial.h" +#include "io/osd.h" #include "rx/rx.h" #include "rx/crsf.h" @@ -241,8 +242,15 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) rxLinkStatistics.uplinkTXPower = crsfPowerStates[linkStats->uplinkTXPower]; rxLinkStatistics.activeAnt = linkStats->activeAntenna; - if (rxLinkStatistics.uplinkLQ > 0) - lqTrackerSet(rxRuntimeConfig->lqTracker, scaleRange(constrain(rxLinkStatistics.uplinkRSSI, -120, -30), -120, -30, 0, RSSI_MAX_VALUE)); + if (rxLinkStatistics.uplinkLQ > 0) { + int16_t uplinkStrength; // RSSI dBm converted to % + uplinkStrength = constrain((100 * ((osdConfig()->rssi_dbm_max - osdConfig()->rssi_dbm_min) * (osdConfig()->rssi_dbm_max - osdConfig()->rssi_dbm_min)) - (osdConfig()->rssi_dbm_max - rxLinkStatistics.uplinkRSSI) * (100 * (osdConfig()->rssi_dbm_max - rxLinkStatistics.uplinkRSSI))) / ((osdConfig()->rssi_dbm_max - osdConfig()->rssi_dbm_min) * (osdConfig()->rssi_dbm_max - osdConfig()->rssi_dbm_min)),0,100); + if (rxLinkStatistics.uplinkRSSI >= osdConfig()->rssi_dbm_max ) + uplinkStrength = 99; + else if (rxLinkStatistics.uplinkRSSI < osdConfig()->rssi_dbm_min) + uplinkStrength = 0; + lqTrackerSet(rxRuntimeConfig->lqTracker, scaleRange(uplinkStrength, 0, 99, 0, RSSI_MAX_VALUE)); + } else lqTrackerSet(rxRuntimeConfig->lqTracker, 0); From dd75e13a82b1c8976efc31d75862c6184dbcfbfa Mon Sep 17 00:00:00 2001 From: OptimusTi Date: Thu, 25 Nov 2021 22:30:06 -0500 Subject: [PATCH 06/17] Update settings.yaml --- src/main/fc/settings.yaml | 30 ++++++++++++++---------------- 1 file changed, 14 insertions(+), 16 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index e5018b0fb3..c40a8380c4 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3221,6 +3221,20 @@ groups: field: rssi_dbm_alarm min: -130 max: 0 + - name: osd_rssi_dbm_max + condition: USE_SERIALRX_CRSF + description: "RSSI dBm upper end of curve. Perfect rssi (max) = 100%" + default_value: -30 + field: rssi_dbm_max + min: -50 + max: 0 + - name: osd_rssi_dbm_min + condition: USE_SERIALRX_CRSF + description: "RSSI dBm lower end of curve or RX sensitivity level. Worst rssi (min) = 0%" + default_value: -120 + field: rssi_dbm_min + min: -130 + max: 0 - name: osd_temp_label_align description: "Allows to chose between left and right alignment for the OSD temperature sensor labels. Valid values are `LEFT` and `RIGHT`" default_value: "LEFT" @@ -3486,22 +3500,6 @@ groups: max: 6 default_value: 3 - - name: osd_rssi_dbm_max - condition: USE_SERIALRX_CRSF - description: "RSSI dBm upper end of curve. Perfect rssi (max) = 100%" - default_value: -30 - field: rssi_dbm_max - min: -50 - max: 0 - - - name: osd_rssi_dbm_min - condition: USE_SERIALRX_CRSF - description: "RSSI dBm lower end of curve or RX sensitivity level. Worst rssi (min) = 0%" - default_value: -120 - field: rssi_dbm_min - min: -130 - max: 0 - - name: PG_OSD_COMMON_CONFIG type: osdCommonConfig_t headers: ["io/osd_common.h"] From 5d9e866fcdbcb676dd404591bea20d1903b6f161 Mon Sep 17 00:00:00 2001 From: OptimusTi Date: Thu, 25 Nov 2021 22:42:05 -0500 Subject: [PATCH 07/17] Update Settings.md --- docs/Settings.md | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/docs/Settings.md b/docs/Settings.md index d5409bfbb9..0bfecfde89 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4492,6 +4492,26 @@ RSSI dBm indicator blinks below this value [dBm]. 0 disables this alarm --- +### osd_rssi_dbm_max + +RSSI dBm upper end of curve. Perfect rssi (max) = 100% + +| Default | Min | Max | +| --- | --- | --- | +| -30 | -50 | 0 | + +--- + +### osd_rssi_dbm_min + +RSSI dBm lower end of curve or RX sensitivity level. Worst rssi (min) = 0% + +| Default | Min | Max | +| --- | --- | --- | +| -120 | -130 | 0 | + +--- + ### osd_sidebar_height Height of sidebars in rows. 0 leaves only the level indicator arrows (Not for pixel OSD) From 06d278825fc988e4d67d82136b0e3f91ccd50281 Mon Sep 17 00:00:00 2001 From: OptimusTi Date: Mon, 29 Nov 2021 17:06:46 -0500 Subject: [PATCH 08/17] Update crsf.c Removed extra () --- src/main/rx/crsf.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c index 705abd9308..aa29b2a410 100755 --- a/src/main/rx/crsf.c +++ b/src/main/rx/crsf.c @@ -244,7 +244,6 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) if (rxLinkStatistics.uplinkLQ > 0) { int16_t uplinkStrength; // RSSI dBm converted to % - uplinkStrength = constrain((100 * ((osdConfig()->rssi_dbm_max - osdConfig()->rssi_dbm_min) * (osdConfig()->rssi_dbm_max - osdConfig()->rssi_dbm_min)) - (osdConfig()->rssi_dbm_max - rxLinkStatistics.uplinkRSSI) * (100 * (osdConfig()->rssi_dbm_max - rxLinkStatistics.uplinkRSSI))) / ((osdConfig()->rssi_dbm_max - osdConfig()->rssi_dbm_min) * (osdConfig()->rssi_dbm_max - osdConfig()->rssi_dbm_min)),0,100); if (rxLinkStatistics.uplinkRSSI >= osdConfig()->rssi_dbm_max ) uplinkStrength = 99; else if (rxLinkStatistics.uplinkRSSI < osdConfig()->rssi_dbm_min) From dd537e5a23c6dd65f58a8e18bafb44f2e81e2fa3 Mon Sep 17 00:00:00 2001 From: OptimusTi Date: Mon, 29 Nov 2021 18:30:00 -0500 Subject: [PATCH 09/17] Update crsf.c --- src/main/rx/crsf.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c index aa29b2a410..71c42b05a3 100755 --- a/src/main/rx/crsf.c +++ b/src/main/rx/crsf.c @@ -244,6 +244,7 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) if (rxLinkStatistics.uplinkLQ > 0) { int16_t uplinkStrength; // RSSI dBm converted to % + uplinkStrength = constrain((100 * sq((osdConfig()->rssi_dbm_max - osdConfig()->rssi_dbm_min)) - (osdConfig()->rssi_dbm_max - rxLinkStatistics.uplinkRSSI) * (100 * (osdConfig()->rssi_dbm_max - rxLinkStatistics.uplinkRSSI))) / sq((osdConfig()->rssi_dbm_max - osdConfig()->rssi_dbm_min)),0,100); if (rxLinkStatistics.uplinkRSSI >= osdConfig()->rssi_dbm_max ) uplinkStrength = 99; else if (rxLinkStatistics.uplinkRSSI < osdConfig()->rssi_dbm_min) From 1c13407abab18e1d87eaec642b995d0f3cfa9293 Mon Sep 17 00:00:00 2001 From: OptimusTi Date: Wed, 1 Dec 2021 17:14:58 -0500 Subject: [PATCH 10/17] Update crsf.c --- src/main/rx/crsf.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c index 71c42b05a3..01b9a72abd 100755 --- a/src/main/rx/crsf.c +++ b/src/main/rx/crsf.c @@ -244,7 +244,7 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) if (rxLinkStatistics.uplinkLQ > 0) { int16_t uplinkStrength; // RSSI dBm converted to % - uplinkStrength = constrain((100 * sq((osdConfig()->rssi_dbm_max - osdConfig()->rssi_dbm_min)) - (osdConfig()->rssi_dbm_max - rxLinkStatistics.uplinkRSSI) * (100 * (osdConfig()->rssi_dbm_max - rxLinkStatistics.uplinkRSSI))) / sq((osdConfig()->rssi_dbm_max - osdConfig()->rssi_dbm_min)),0,100); + uplinkStrength = constrain((100 * sq((osdConfig()->rssi_dbm_max - osdConfig()->rssi_dbm_min)) - (100 * sq((osdConfig()->rssi_dbm_max - rxLinkStatistics.uplinkRSSI)))) / sq((osdConfig()->rssi_dbm_max - osdConfig()->rssi_dbm_min)),0,100); if (rxLinkStatistics.uplinkRSSI >= osdConfig()->rssi_dbm_max ) uplinkStrength = 99; else if (rxLinkStatistics.uplinkRSSI < osdConfig()->rssi_dbm_min) From 643412f53fd7b5a6b202f99b91d789e6c9a88faf Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 23 Dec 2021 12:24:09 +0100 Subject: [PATCH 11/17] Regenerate docs --- docs/Settings.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index f126548cfb..77690c30be 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -5708,7 +5708,7 @@ Enable the alternate softserial method. This is the method used in iNav 3.0 and | Default | Min | Max | | --- | --- | --- | -| ON | | | +| ON | OFF | ON | --- From 90996a97438796c01c0beddd30d6b966f2a6d547 Mon Sep 17 00:00:00 2001 From: bkleiner Date: Mon, 20 Dec 2021 23:50:15 +0100 Subject: [PATCH 12/17] KAKUTEH7: add spi sdcard pin defintions --- src/main/target/KAKUTEH7/target.h | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/main/target/KAKUTEH7/target.h b/src/main/target/KAKUTEH7/target.h index ade11c62aa..9b8727abd1 100644 --- a/src/main/target/KAKUTEH7/target.h +++ b/src/main/target/KAKUTEH7/target.h @@ -41,6 +41,15 @@ #define SPI1_MISO_PIN PA6 #define SPI1_MOSI_PIN PA7 +#define USE_SDCARD +#define USE_SDCARD_SPI +#define SDCARD_SPI_BUS BUS_SPI1 +#define SDCARD_CS_PIN PA4 +#define SDCARD_DETECT_INVERTED +#define SDCARD_DETECT_PIN PA3 + +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + // *************** SPI2 *********************** #define USE_SPI_DEVICE_2 #define SPI2_SCK_PIN PB13 @@ -148,7 +157,7 @@ #define USE_LED_STRIP #define WS2811_PIN PD12 -#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL) +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) #define CURRENT_METER_SCALE 250 #define USE_SERIAL_4WAY_BLHELI_INTERFACE From d7341025b8017a3be2a32b425a9071f5dbed0709 Mon Sep 17 00:00:00 2001 From: bkleiner Date: Mon, 20 Dec 2021 23:57:28 +0100 Subject: [PATCH 13/17] add h7 support to spi ll driver, drop hal version --- cmake/stm32h7.cmake | 4 +- src/main/drivers/bus_spi_hal.c | 239 ------------------------------ src/main/drivers/bus_spi_hal_ll.c | 95 +++++++++--- 3 files changed, 78 insertions(+), 260 deletions(-) delete mode 100644 src/main/drivers/bus_spi_hal.c diff --git a/cmake/stm32h7.cmake b/cmake/stm32h7.cmake index c936bf0dbc..70400fb9f0 100644 --- a/cmake/stm32h7.cmake +++ b/cmake/stm32h7.cmake @@ -114,7 +114,7 @@ set(STM32H7_HAL_SRC # stm32h7xx_ll_rng.c # stm32h7xx_ll_rtc.c stm32h7xx_ll_sdmmc.c -# stm32h7xx_ll_spi.c + stm32h7xx_ll_spi.c # stm32h7xx_ll_swpmi.c stm32h7xx_ll_tim.c # stm32h7xx_ll_usart.c @@ -148,7 +148,7 @@ main_sources(STM32H7_SRC drivers/adc_stm32h7xx.c drivers/bus_i2c_hal.c drivers/dma_stm32h7xx.c - drivers/bus_spi_hal.c + drivers/bus_spi_hal_ll.c drivers/memprot.h drivers/memprot_hal.c drivers/memprot_stm32h7xx.c diff --git a/src/main/drivers/bus_spi_hal.c b/src/main/drivers/bus_spi_hal.c deleted file mode 100644 index 748dd25daa..0000000000 --- a/src/main/drivers/bus_spi_hal.c +++ /dev/null @@ -1,239 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight 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. - * - * Cleanflight 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 Cleanflight. If not, see . - */ - -#include -#include -#include - -#include - -#include "build/debug.h" -#include "drivers/bus_spi.h" -#include "dma.h" -#include "drivers/io.h" -#include "io_impl.h" -#include "drivers/nvic.h" -#include "rcc.h" - -#ifndef SPI1_NSS_PIN -#define SPI1_NSS_PIN NONE -#endif -#ifndef SPI2_NSS_PIN -#define SPI2_NSS_PIN NONE -#endif -#ifndef SPI3_NSS_PIN -#define SPI3_NSS_PIN NONE -#endif -#ifndef SPI4_NSS_PIN -#define SPI4_NSS_PIN NONE -#endif - -#if defined(USE_SPI_DEVICE_1) -static const uint32_t spiDivisorMapFast[] = { - SPI_BAUDRATEPRESCALER_256, // SPI_CLOCK_INITIALIZATON 421.875 KBits/s - SPI_BAUDRATEPRESCALER_32, // SPI_CLOCK_SLOW 843.75 KBits/s - SPI_BAUDRATEPRESCALER_16, // SPI_CLOCK_STANDARD 6.75 MBits/s - SPI_BAUDRATEPRESCALER_8, // SPI_CLOCK_FAST 13.5 MBits/s - SPI_BAUDRATEPRESCALER_4 // SPI_CLOCK_ULTRAFAST 27.0 MBits/s -}; -#endif - -#if defined(USE_SPI_DEVICE_2) || defined(USE_SPI_DEVICE_3) || defined(USE_SPI_DEVICE_4) -static const uint32_t spiDivisorMapSlow[] = { - SPI_BAUDRATEPRESCALER_256, // SPI_CLOCK_INITIALIZATON 210.937 KBits/s - SPI_BAUDRATEPRESCALER_64, // SPI_CLOCK_SLOW 843.75 KBits/s - SPI_BAUDRATEPRESCALER_8, // SPI_CLOCK_STANDARD 6.75 MBits/s - SPI_BAUDRATEPRESCALER_4, // SPI_CLOCK_FAST 13.5 MBits/s - SPI_BAUDRATEPRESCALER_2 // SPI_CLOCK_ULTRAFAST 27.0 MBits/s -}; -#endif - -static spiDevice_t spiHardwareMap[SPIDEV_COUNT] = { -#ifdef USE_SPI_DEVICE_1 - { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF5_SPI1, .divisorMap = spiDivisorMapFast }, -#else - { .dev = NULL }, // No SPI1 -#endif -#ifdef USE_SPI_DEVICE_2 - { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1L(SPI2), .af = GPIO_AF5_SPI2, .divisorMap = spiDivisorMapSlow }, -#else - { .dev = NULL }, // No SPI2 -#endif -#ifdef USE_SPI_DEVICE_3 - { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1L(SPI3), .af = GPIO_AF6_SPI3, .divisorMap = spiDivisorMapSlow }, -#else - { .dev = NULL }, // No SPI3 -#endif -#ifdef USE_SPI_DEVICE_4 - { .dev = SPI4, .nss = IO_TAG(SPI4_NSS_PIN), .sck = IO_TAG(SPI4_SCK_PIN), .miso = IO_TAG(SPI4_MISO_PIN), .mosi = IO_TAG(SPI4_MOSI_PIN), .rcc = RCC_APB2(SPI4), .af = GPIO_AF5_SPI4, .divisorMap = spiDivisorMapSlow } -#else - { .dev = NULL } // No SPI4 -#endif -}; - -static SPI_HandleTypeDef spiHandle[SPIDEV_COUNT]; - -SPIDevice spiDeviceByInstance(SPI_TypeDef *instance) -{ - if (instance == SPI1) - return SPIDEV_1; - - if (instance == SPI2) - return SPIDEV_2; - - if (instance == SPI3) - return SPIDEV_3; - - if (instance == SPI4) - return SPIDEV_4; - - return SPIINVALID; -} - -void spiTimeoutUserCallback(SPI_TypeDef *instance) -{ - SPIDevice device = spiDeviceByInstance(instance); - if (device == SPIINVALID) { - return; - } - - spiHardwareMap[device].errorCount++; -} - -bool spiInitDevice(SPIDevice device, bool leadingEdge) -{ - spiDevice_t *spi = &spiHardwareMap[device]; - - if (!spi->dev) { - return false; - } - - if (spi->initDone) { - return true; - } - - // Enable SPI clock - RCC_ClockCmd(spi->rcc, ENABLE); - RCC_ResetCmd(spi->rcc, DISABLE); - - IOInit(IOGetByTag(spi->sck), OWNER_SPI, RESOURCE_SPI_SCK, device + 1); - IOInit(IOGetByTag(spi->miso), OWNER_SPI, RESOURCE_SPI_MISO, device + 1); - IOInit(IOGetByTag(spi->mosi), OWNER_SPI, RESOURCE_SPI_MOSI, device + 1); - - if (leadingEdge) { - IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_LOW, spi->af); - } else { - IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_HIGH, spi->af); - } - IOConfigGPIOAF(IOGetByTag(spi->miso), SPI_IO_AF_MISO_CFG, spi->af); - IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->af); - - if (spi->nss) { - IOInit(IOGetByTag(spi->nss), OWNER_SPI, RESOURCE_SPI_CS, device + 1); - IOConfigGPIO(IOGetByTag(spi->nss), SPI_IO_CS_CFG); - } - - SPI_HandleTypeDef * hspi = &spiHandle[device]; - memset(hspi, 0, sizeof(SPI_HandleTypeDef)); - hspi->Instance = spi->dev; - - HAL_SPI_DeInit(hspi); - - hspi->Init.Mode = SPI_MODE_MASTER; - hspi->Init.Direction = SPI_DIRECTION_2LINES; - hspi->Init.DataSize = SPI_DATASIZE_8BIT; - hspi->Init.NSS = SPI_NSS_SOFT; - hspi->Init.FirstBit = SPI_FIRSTBIT_MSB; - hspi->Init.CRCPolynomial = 7; - hspi->Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_256; - hspi->Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE; - hspi->Init.TIMode = SPI_TIMODE_DISABLED; - hspi->Init.FifoThreshold = SPI_FIFO_THRESHOLD_01DATA; - hspi->Init.MasterKeepIOState = SPI_MASTER_KEEP_IO_STATE_ENABLE; /* Recommanded setting to avoid glitches */ - - if (leadingEdge) { - hspi->Init.CLKPolarity = SPI_POLARITY_LOW; - hspi->Init.CLKPhase = SPI_PHASE_1EDGE; - } - else { - hspi->Init.CLKPolarity = SPI_POLARITY_HIGH; - hspi->Init.CLKPhase = SPI_PHASE_2EDGE; - } - - if (spi->nss) { - IOHi(IOGetByTag(spi->nss)); - } - - HAL_SPI_Init(hspi); - - spi->initDone = true; - return true; -} - -uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t txByte) -{ - uint8_t rxData; - spiTransfer(instance, &rxData, &txByte, 1); - return rxData; -} - -/** - * Return true if the bus is currently in the middle of a transmission. - */ -bool spiIsBusBusy(SPI_TypeDef *instance) -{ - SPIDevice device = spiDeviceByInstance(instance); - return (spiHandle[device].State == HAL_SPI_STATE_BUSY); -} - -bool spiTransfer(SPI_TypeDef *instance, uint8_t *rxData, const uint8_t *txData, int len) -{ - SPIDevice device = spiDeviceByInstance(instance); - SPI_HandleTypeDef * hspi = &spiHandle[device]; - HAL_StatusTypeDef status; - - #define SPI_DEFAULT_TIMEOUT 10 - - if (!rxData) { - status = HAL_SPI_Transmit(hspi, txData, len, SPI_DEFAULT_TIMEOUT); - } else if(!txData) { - status = HAL_SPI_Receive(hspi, rxData, len, SPI_DEFAULT_TIMEOUT); - } else { - status = HAL_SPI_TransmitReceive(hspi, txData, rxData, len, SPI_DEFAULT_TIMEOUT); - } - - if(status != HAL_OK) { - spiTimeoutUserCallback(instance); - } - - return true; -} - -void spiSetSpeed(SPI_TypeDef *instance, SPIClockSpeed_e speed) -{ - SPIDevice device = spiDeviceByInstance(instance); - SPI_HandleTypeDef * hspi = &spiHandle[device]; - - HAL_SPI_DeInit(hspi); - hspi->Init.BaudRatePrescaler = spiHardwareMap[device].divisorMap[speed]; - HAL_SPI_Init(hspi); -} - -SPI_TypeDef * spiInstanceByDevice(SPIDevice device) -{ - return spiHardwareMap[device].dev; -} diff --git a/src/main/drivers/bus_spi_hal_ll.c b/src/main/drivers/bus_spi_hal_ll.c index 9c02ae2ccc..55053bc60c 100644 --- a/src/main/drivers/bus_spi_hal_ll.c +++ b/src/main/drivers/bus_spi_hal_ll.c @@ -88,6 +88,30 @@ static const uint32_t spiDivisorMapSlow[] = { }; #endif +#if defined(STM32H7) +static spiDevice_t spiHardwareMap[SPIDEV_COUNT] = { +#ifdef USE_SPI_DEVICE_1 + { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF5_SPI1, .divisorMap = spiDivisorMapFast }, +#else + { .dev = NULL }, // No SPI1 +#endif +#ifdef USE_SPI_DEVICE_2 + { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1L(SPI2), .af = GPIO_AF5_SPI2, .divisorMap = spiDivisorMapSlow }, +#else + { .dev = NULL }, // No SPI2 +#endif +#ifdef USE_SPI_DEVICE_3 + { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1L(SPI3), .af = GPIO_AF6_SPI3, .divisorMap = spiDivisorMapSlow }, +#else + { .dev = NULL }, // No SPI3 +#endif +#ifdef USE_SPI_DEVICE_4 + { .dev = SPI4, .nss = IO_TAG(SPI4_NSS_PIN), .sck = IO_TAG(SPI4_SCK_PIN), .miso = IO_TAG(SPI4_MISO_PIN), .mosi = IO_TAG(SPI4_MOSI_PIN), .rcc = RCC_APB2(SPI4), .af = GPIO_AF5_SPI4, .divisorMap = spiDivisorMapSlow } +#else + { .dev = NULL } // No SPI4 +#endif +}; +#else static spiDevice_t spiHardwareMap[] = { #ifdef USE_SPI_DEVICE_1 { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF5_SPI1, .divisorMap = spiDivisorMapFast }, @@ -110,6 +134,7 @@ static spiDevice_t spiHardwareMap[] = { { .dev = NULL } // No SPI4 #endif }; +#endif SPIDevice spiDeviceByInstance(SPI_TypeDef *instance) { @@ -187,12 +212,21 @@ bool spiInitDevice(SPIDevice device, bool leadingEdge) .CRCPoly = 7, .CRCCalculation = SPI_CRCCALCULATION_DISABLE, }; + +#if defined(STM32H7) + // Prevent glitching when SPI is disabled + LL_SPI_EnableGPIOControl(spi->dev); + + LL_SPI_SetFIFOThreshold(spi->dev, LL_SPI_FIFO_TH_01DATA); + LL_SPI_Init(spi->dev, &init); +#else LL_SPI_SetRxFIFOThreshold(spi->dev, SPI_RXFIFO_THRESHOLD_QF); LL_SPI_Init(spi->dev, &init); LL_SPI_Enable(spi->dev); SET_BIT(spi->dev->CR2, SPI_RXFIFO_THRESHOLD); +#endif if (spi->nss) { IOHi(IOGetByTag(spi->nss)); @@ -204,26 +238,11 @@ bool spiInitDevice(SPIDevice device, bool leadingEdge) uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t txByte) { - uint16_t spiTimeout = 1000; - - while (!LL_SPI_IsActiveFlag_TXE(instance)) { - if ((spiTimeout--) == 0) { - spiTimeoutUserCallback(instance); - return 0xFF; - } + uint8_t value = 0xFF; + if (!spiTransfer(instance, &value, &txByte, 1)) { + return 0xFF; } - - LL_SPI_TransmitData8(instance, txByte); - - spiTimeout = 1000; - while (!LL_SPI_IsActiveFlag_RXNE(instance)) { - if ((spiTimeout--) == 0) { - spiTimeoutUserCallback(instance); - return 0xFF; - } - } - - return (uint8_t)LL_SPI_ReceiveData8(instance); + return value; } /** @@ -231,11 +250,48 @@ uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t txByte) */ bool spiIsBusBusy(SPI_TypeDef *instance) { +#if defined(STM32H7) + // H7 doesnt really have a busy flag. its should be done when the transfer is. + return false; +#else return (LL_SPI_GetTxFIFOLevel(instance) != LL_SPI_TX_FIFO_EMPTY) || LL_SPI_IsActiveFlag_BSY(instance); +#endif } bool spiTransfer(SPI_TypeDef *instance, uint8_t *rxData, const uint8_t *txData, int len) { +#if defined(STM32H7) + LL_SPI_SetTransferSize(instance, len); + LL_SPI_Enable(instance); + LL_SPI_StartMasterTransfer(instance); + while (len) { + int spiTimeout = 1000; + while(!LL_SPI_IsActiveFlag_TXP(instance)) { + if ((spiTimeout--) == 0) { + spiTimeoutUserCallback(instance); + return false; + } + } + uint8_t b = txData ? *(txData++) : 0xFF; + LL_SPI_TransmitData8(instance, b); + + spiTimeout = 1000; + while (!LL_SPI_IsActiveFlag_RXP(instance)) { + if ((spiTimeout--) == 0) { + spiTimeoutUserCallback(instance); + return false; + } + } + b = LL_SPI_ReceiveData8(instance); + if (rxData) { + *(rxData++) = b; + } + --len; + } + while (!LL_SPI_IsActiveFlag_EOT(instance)); + LL_SPI_ClearFlag_TXTF(instance); + LL_SPI_Disable(instance); +#else SET_BIT(instance->CR2, SPI_RXFIFO_THRESHOLD); while (len) { @@ -262,6 +318,7 @@ bool spiTransfer(SPI_TypeDef *instance, uint8_t *rxData, const uint8_t *txData, } --len; } +#endif return true; } From 1de659ba669b3655348ad500e02f4816eccfe1b9 Mon Sep 17 00:00:00 2001 From: bkleiner Date: Thu, 23 Dec 2021 17:50:21 +0100 Subject: [PATCH 14/17] spi: remove unused hal function declrations --- src/main/drivers/bus_spi.h | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index 24dc486038..2f5f8e0be7 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -84,9 +84,4 @@ bool spiTransfer(SPI_TypeDef *instance, uint8_t *rxData, const uint8_t *txData, uint16_t spiGetErrorCounter(SPI_TypeDef *instance); void spiResetErrorCounter(SPI_TypeDef *instance); SPIDevice spiDeviceByInstance(SPI_TypeDef *instance); -SPI_TypeDef * spiInstanceByDevice(SPIDevice device); - -#if defined(USE_HAL_DRIVER) -SPI_HandleTypeDef* spiHandleByInstance(SPI_TypeDef *instance); -DMA_HandleTypeDef* spiSetDMATransmit(DMA_Stream_TypeDef *Stream, uint32_t Channel, SPI_TypeDef *Instance, uint8_t *pData, uint16_t Size); -#endif +SPI_TypeDef * spiInstanceByDevice(SPIDevice device); \ No newline at end of file From a445baa705e5032dfa9aea432eac70aaefae3162 Mon Sep 17 00:00:00 2001 From: bkleiner Date: Thu, 23 Dec 2021 17:57:50 +0100 Subject: [PATCH 15/17] stm32 LL library: fix pre-compiler warning --- .../STM32H7/Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_ll_spi.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/lib/main/STM32H7/Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_ll_spi.c b/lib/main/STM32H7/Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_ll_spi.c index 44a95c1256..05a6e5e993 100644 --- a/lib/main/STM32H7/Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_ll_spi.c +++ b/lib/main/STM32H7/Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_ll_spi.c @@ -28,8 +28,10 @@ #ifdef USE_FULL_ASSERT #include "stm32_assert.h" #else +#ifndef assert_param #define assert_param(expr) ((void)0U) #endif +#endif /** @addtogroup STM32H7xx_LL_Driver * @{ From 6a66be2d4540c9046d9b0676fecf02a7772c615d Mon Sep 17 00:00:00 2001 From: bkleiner Date: Thu, 23 Dec 2021 18:05:37 +0100 Subject: [PATCH 16/17] spi: fix unused parameter --- src/main/drivers/bus_spi_hal_ll.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/drivers/bus_spi_hal_ll.c b/src/main/drivers/bus_spi_hal_ll.c index 55053bc60c..25662f2f2d 100644 --- a/src/main/drivers/bus_spi_hal_ll.c +++ b/src/main/drivers/bus_spi_hal_ll.c @@ -251,6 +251,7 @@ uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t txByte) bool spiIsBusBusy(SPI_TypeDef *instance) { #if defined(STM32H7) + UNUSED(instance); // H7 doesnt really have a busy flag. its should be done when the transfer is. return false; #else From 53923e9271ca3df53dddc80a498131cc333d532b Mon Sep 17 00:00:00 2001 From: Olivier C Date: Mon, 3 Jan 2022 19:51:07 +0100 Subject: [PATCH 17/17] Changing the UP and DOWN symbols for the radar relative altitude display The OSD font was changed before the PR was merged --- src/main/io/osd_hud.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/osd_hud.c b/src/main/io/osd_hud.c index ce87370bcd..62c8187c6b 100644 --- a/src/main/io/osd_hud.c +++ b/src/main/io/osd_hud.c @@ -205,7 +205,7 @@ void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitu if (((millis() / 1000) % 6 == 0) && poiType > 0) { // For Radar and WPs, display the difference in altitude altc = ((osd_unit_e)osdConfig()->units == OSD_UNIT_IMPERIAL) ? constrain(CENTIMETERS_TO_FEET(poiAltitude * 100), -99, 99) : constrain(poiAltitude, -99 , 99); tfp_sprintf(buff, "%3d", altc); - buff[0] = (poiAltitude >= 0) ? SYM_VARIO_UP_2A : SYM_VARIO_DOWN_2A; + buff[0] = (poiAltitude >= 0) ? SYM_HUD_ARROWS_U3 : SYM_HUD_ARROWS_D3; } else { // Display the distance by default if ((osd_unit_e)osdConfig()->units == OSD_UNIT_IMPERIAL) {