diff --git a/docs/OSD.md b/docs/OSD.md index d57da7e25f..5787496687 100644 --- a/docs/OSD.md +++ b/docs/OSD.md @@ -2,6 +2,15 @@ The On Screen Display, or OSD, is a feature that overlays flight data over the video image. This can be done on the flight controller, using the analogue MAX7456 chip. Digital systems take the OSD data, via MSP DisplayPort, send it to the video receiver; which combines the data with the image. You can specify what elements are displayed, and their locations on the image. Most systems are character based, and use the MAX7456 analogue setup, or MSP DisplayPort. However, there are some different systems which are also supported. Such as the canvas based FrSKY PixelOSD on analogue. Canvas OSDs draw shapes on the image. Whereas character based OSDs use font characters to display the data. + +General OSD information is in this document. Other documents cover specific OSD-related topics: +* [Custom OSD Messages](https://github.com/iNavFlight/inav/wiki/OSD-custom-messages) +* [OSD Hud and ESP32 radars](https://github.com/iNavFlight/inav/wiki/OSD-Hud-and-ESP32-radars) +* [OSD Joystick](https://github.com/iNavFlight/inav/blob/master/docs/OSD%20Joystick.md) +* [DJI compatible OSD.md](https://github.com/iNavFlight/inav/blob/master/docs/DJI%20compatible%20OSD.md) +* [Pixel OSD FAQ](https://github.com/iNavFlight/inav/wiki/Pixel-OSD-FAQs) + + ## Features and Limitations Not all OSDs are created equally. This table shows the differences between the different systems available. diff --git a/docs/Settings.md b/docs/Settings.md index 872c63bc42..a217a26a98 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1782,13 +1782,23 @@ Specifies the type of the software LPF of the gyro signals. --- +### gyro_lulu_enabled + +Enable/disable gyro LULU filter + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + ### gyro_lulu_sample_count Gyro lulu sample count, in number of samples. | Default | Min | Max | | --- | --- | --- | -| 3 | | 15 | +| 3 | 1 | 15 | --- diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 7ee7447154..0d897f7710 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -77,6 +77,7 @@ typedef enum { DEBUG_ADAPTIVE_FILTER, DEBUG_HEADTRACKING, DEBUG_GPS, + DEBUG_LULU, DEBUG_COUNT // also update debugModeNames in cli.c } debugType_e; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 278be34433..6670148d0b 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -84,7 +84,7 @@ tables: "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "NAV_YAW", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "ALTITUDE", "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST", - "ADAPTIVE_FILTER", "HEADTRACKER", "GPS" ] + "ADAPTIVE_FILTER", "HEADTRACKER", "GPS", "LULU"] - name: aux_operator values: ["OR", "AND"] enum: modeActivationOperator_e @@ -193,7 +193,7 @@ tables: values: ["SHARED_LOW", "SHARED_HIGH", "LOW", "HIGH"] enum: led_pin_pwm_mode_e - name: gyro_filter_mode - values: ["STATIC", "DYNAMIC", "ADAPTIVE", "LULU"] + values: ["OFF", "STATIC", "DYNAMIC", "ADAPTIVE"] enum: gyroFilterType_e - name: headtracker_dev_type values: ["NONE", "SERIAL", "MSP"] @@ -227,10 +227,16 @@ groups: default_value: 250 field: gyro_anti_aliasing_lpf_hz max: 1000 + - name: gyro_lulu_enabled + description: "Enable/disable gyro LULU filter" + default_value: OFF + field: gyroLuluEnabled + type: bool - name: gyro_lulu_sample_count description: "Gyro lulu sample count, in number of samples." default_value: 3 field: gyroLuluSampleCount + min: 1 max: 15 - name: gyro_main_lpf_hz description: "Software based gyro main lowpass filter. Value is cutoff frequency (Hz)" diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index b4723f0da7..9fa0a7c0e4 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -88,6 +88,9 @@ STATIC_FASTRAM filter_t gyroLpfState[XYZ_AXIS_COUNT]; STATIC_FASTRAM filterApplyFnPtr gyroLpf2ApplyFn; STATIC_FASTRAM filter_t gyroLpf2State[XYZ_AXIS_COUNT]; +STATIC_FASTRAM filterApplyFnPtr gyroLuluApplyFn; +STATIC_FASTRAM filter_t gyroLuluState[XYZ_AXIS_COUNT]; + #ifdef USE_DYNAMIC_FILTERS EXTENDED_FASTRAM gyroAnalyseState_t gyroAnalyseState; @@ -96,7 +99,7 @@ EXTENDED_FASTRAM secondaryDynamicGyroNotchState_t secondaryDynamicGyroNotchState #endif -PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 11); +PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 12); PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_anti_aliasing_lpf_hz = SETTING_GYRO_ANTI_ALIASING_LPF_HZ_DEFAULT, @@ -132,7 +135,8 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .adaptiveFilterIntegratorThresholdLow = SETTING_GYRO_ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD_LOW_DEFAULT, #endif .gyroFilterMode = SETTING_GYRO_FILTER_MODE_DEFAULT, - .gyroLuluSampleCount = SETTING_GYRO_LULU_SAMPLE_COUNT_DEFAULT + .gyroLuluSampleCount = SETTING_GYRO_LULU_SAMPLE_COUNT_DEFAULT, + .gyroLuluEnabled = SETTING_GYRO_LULU_ENABLED_DEFAULT ); STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware) @@ -241,18 +245,13 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard return gyroHardware; } -static void initGyroFilter(filterApplyFnPtr *applyFn, filter_t state[], uint16_t cutoff, uint32_t looptime, filterType_e filterType) +static void initGyroFilter(filterApplyFnPtr *applyFn, filter_t state[], uint16_t cutoff, uint32_t looptime) { *applyFn = nullFilterApply; if (cutoff > 0) { + *applyFn = (filterApplyFnPtr)pt1FilterApply; for (int axis = 0; axis < 3; axis++) { - if(filterType == FILTER_LULU) { - luluFilterInit(&state[axis].lulu, cutoff); - *applyFn = (filterApplyFnPtr)luluFilterApply; - } else { - pt1FilterInit(&state[axis].pt1, cutoff, US2S(looptime)); - *applyFn = (filterApplyFnPtr)pt1FilterApply; - } + pt1FilterInit(&state[axis].pt1, cutoff, US2S(looptime)); } } } @@ -260,13 +259,22 @@ static void initGyroFilter(filterApplyFnPtr *applyFn, filter_t state[], uint16_t static void gyroInitFilters(void) { //First gyro LPF running at full gyro frequency 8kHz - initGyroFilter(&gyroLpfApplyFn, gyroLpfState, gyroConfig()->gyro_anti_aliasing_lpf_hz, getGyroLooptime(), FILTER_PT1); + initGyroFilter(&gyroLpfApplyFn, gyroLpfState, gyroConfig()->gyro_anti_aliasing_lpf_hz, getGyroLooptime()); - if(gyroConfig()->gyroFilterMode == GYRO_FILTER_MODE_LULU) { - initGyroFilter(&gyroLpf2ApplyFn, gyroLpf2State, gyroConfig()->gyroLuluSampleCount, getLooptime(), FILTER_LULU); + if (gyroConfig()->gyroLuluEnabled && gyroConfig()->gyroLuluSampleCount > 0) { + gyroLuluApplyFn = (filterApplyFnPtr)luluFilterApply; + + for (int axis = 0; axis < 3; axis++) { + luluFilterInit(&gyroLuluState[axis].lulu, gyroConfig()->gyroLuluSampleCount); + } } else { - //Second gyro LPF runnig and PID frequency - this filter is dynamic when gyro_use_dyn_lpf = ON - initGyroFilter(&gyroLpf2ApplyFn, gyroLpf2State, gyroConfig()->gyro_main_lpf_hz, getLooptime(), FILTER_PT1); + gyroLuluApplyFn = nullFilterApply; + } + + if (gyroConfig()->gyroFilterMode != GYRO_FILTER_MODE_OFF) { + initGyroFilter(&gyroLpf2ApplyFn, gyroLpf2State, gyroConfig()->gyro_main_lpf_hz, getLooptime()); + } else { + gyroLpf2ApplyFn = nullFilterApply; } #ifdef USE_ADAPTIVE_FILTER @@ -463,6 +471,17 @@ void FAST_CODE NOINLINE gyroFilter(void) gyroADCf = rpmFilterGyroApply(axis, gyroADCf); #endif + // LULU gyro filter + DEBUG_SET(DEBUG_LULU, axis, gyroADCf); //Pre LULU debug + float preLulu = gyroADCf; + gyroADCf = gyroLuluApplyFn((filter_t *) &gyroLuluState[axis], gyroADCf); + DEBUG_SET(DEBUG_LULU, axis + 3, gyroADCf); //Post LULU debug + + if (axis == ROLL) { + DEBUG_SET(DEBUG_LULU, 6, gyroADCf - preLulu); //LULU delta debug + } + + // Gyro Main LPF gyroADCf = gyroLpf2ApplyFn((filter_t *) &gyroLpf2State[axis], gyroADCf); #ifdef USE_ADAPTIVE_FILTER diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 4b67081a5a..de78b2f81c 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -53,10 +53,10 @@ typedef enum { } dynamicGyroNotchMode_e; typedef enum { - GYRO_FILTER_MODE_STATIC = 0, - GYRO_FILTER_MODE_DYNAMIC = 1, - GYRO_FILTER_MODE_ADAPTIVE = 2, - GYRO_FILTER_MODE_LULU = 3 + GYRO_FILTER_MODE_OFF = 0, + GYRO_FILTER_MODE_STATIC = 1, + GYRO_FILTER_MODE_DYNAMIC = 2, + GYRO_FILTER_MODE_ADAPTIVE = 3 } gyroFilterMode_e; typedef struct gyro_s { @@ -105,6 +105,7 @@ typedef struct gyroConfig_s { uint8_t gyroFilterMode; uint8_t gyroLuluSampleCount; + bool gyroLuluEnabled; } gyroConfig_t; PG_DECLARE(gyroConfig_t, gyroConfig);