From 8918a1281e5b6e2fa1e6efb2c070e736bdcc6256 Mon Sep 17 00:00:00 2001 From: "Konstantin Sharlaimov (DigitalEntity)" Date: Mon, 13 Aug 2018 21:52:53 +0200 Subject: [PATCH 1/3] Experimental biquad RC fitler instead of RC interpolation --- docs/Cli.md | 2 +- make/source.mk | 1 + src/main/common/filter.c | 2 +- src/main/common/filter.h | 2 +- src/main/fc/fc_core.c | 44 ++------------- src/main/fc/rc_smoothing.c | 110 +++++++++++++++++++++++++++++++++++++ src/main/fc/rc_smoothing.h | 31 +++++++++++ src/main/fc/settings.yaml | 7 ++- src/main/rx/rx.c | 4 +- src/main/rx/rx.h | 2 +- 10 files changed, 156 insertions(+), 49 deletions(-) create mode 100644 src/main/fc/rc_smoothing.c create mode 100644 src/main/fc/rc_smoothing.h diff --git a/docs/Cli.md b/docs/Cli.md index 58e75d9fc33..19b1cf8b67d 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -104,7 +104,7 @@ After restoring it's always a good idea to `dump` or `diff` the settings once ag | rssi_channel | 0 | RX channel containing the RSSI signal | | rssi_min | 0 | The minimum RSSI value sent by the receiver, in %. For example, if your receiver's minimum RSSI value shows as 42% in the configurator/OSD set this parameter to 42. See also rssi_max. Note that rssi_min can be set to a value bigger than rssi_max to invert the RSSI calculation (i.e. bigger values mean lower RSSI). | | rssi_max | 100 | The maximum RSSI value sent by the receiver, in %. For example, if your receiver's maximum RSSI value shows as 83% in the configurator/OSD set this parameter to 83. See also rssi_min. | -| rc_smoothing | ON | Interpolation of Rc data during looptimes when there are no new updates. This gives smoother RC input to PID controller and cleaner PIDsum | +| rc_filter_frequency | 50 | RC data biquad filter cutoff frequency. Lower cutoff frequencies result in smoother response at expense of command control delay. Practical values are 20-50. Set to zero to disable entirely and use unsmoothed RC stick values | | input_filtering_mode | OFF | Filter out noise from OpenLRS Telemetry RX | | min_throttle | 1150 | These are min/max values (in us) that are sent to esc when armed. Defaults of 1150/1850 are OK for everyone, for use with AfroESC, they could be set to 1064/1864. | | max_throttle | 1850 | These are min/max values (in us) that are sent to esc when armed. Defaults of 1150/1850 are OK for everyone, for use with AfroESC, they could be set to 1064/1864. If you have brushed motors, the value should be set to 2000. | diff --git a/make/source.mk b/make/source.mk index 8addbee8a75..8d483378e7a 100644 --- a/make/source.mk +++ b/make/source.mk @@ -64,6 +64,7 @@ COMMON_SRC = \ fc/fc_hardfaults.c \ fc/fc_msp.c \ fc/fc_msp_box.c \ + fc/rc_smoothing.c \ fc/rc_adjustments.c \ fc/rc_controls.c \ fc/rc_curves.c \ diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 750a43a3a48..1872d81dfdf 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -70,7 +70,7 @@ float pt1FilterApply3(pt1Filter_t *filter, float input, float dT) return filter->state; } -float pt1FilterApply4(pt1Filter_t *filter, float input, uint16_t f_cut, float dT) +float pt1FilterApply4(pt1Filter_t *filter, float input, float f_cut, float dT) { // Pre calculate and store RC if (!filter->RC) { diff --git a/src/main/common/filter.h b/src/main/common/filter.h index a0b56803d93..ae417364379 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -61,7 +61,7 @@ void pt1FilterSetTimeConstant(pt1Filter_t *filter, float tau); float pt1FilterGetLastOutput(pt1Filter_t *filter); float pt1FilterApply(pt1Filter_t *filter, float input); float pt1FilterApply3(pt1Filter_t *filter, float input, float dT); -float pt1FilterApply4(pt1Filter_t *filter, float input, uint16_t f_cut, float dt); +float pt1FilterApply4(pt1Filter_t *filter, float input, float f_cut, float dt); void pt1FilterReset(pt1Filter_t *filter, float input); void rateLimitFilterInit(rateLimitFilter_t *filter); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 6cc082c6959..c2da7bf6d28 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -53,6 +53,7 @@ #include "fc/config.h" #include "fc/controlrate_profile.h" #include "fc/rc_adjustments.h" +#include "fc/rc_smoothing.h" #include "fc/rc_controls.h" #include "fc/rc_curves.h" #include "fc/rc_modes.h" @@ -419,6 +420,7 @@ void processRx(timeUs_t currentTimeUs) { static bool armedBeeperOn = false; + // Calculate RPY channel data calculateRxChannelsAndUpdateFailsafe(currentTimeUs); // in 3D mode, we need to be able to disarm by switch at any time @@ -634,44 +636,6 @@ void processRx(timeUs_t currentTimeUs) } -void filterRc(bool isRXDataNew) -{ - static int16_t lastCommand[4] = { 0, 0, 0, 0 }; - static int16_t deltaRC[4] = { 0, 0, 0, 0 }; - static int16_t factor, rcInterpolationFactor; - static biquadFilter_t filteredCycleTimeState; - static bool filterInitialised; - - // Calculate average cycle time (1Hz LPF on cycle time) - if (!filterInitialised) { - biquadFilterInitLPF(&filteredCycleTimeState, 1, getLooptime()); - filterInitialised = true; - } - - const timeDelta_t filteredCycleTime = biquadFilterApply(&filteredCycleTimeState, (float) cycleTime); - rcInterpolationFactor = rxGetRefreshRate() / filteredCycleTime + 1; - - if (isRXDataNew) { - for (int channel=0; channel < 4; channel++) { - deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor); - lastCommand[channel] = rcCommand[channel]; - } - - factor = rcInterpolationFactor - 1; - } else { - factor--; - } - - // Interpolate steps of rcCommand - if (factor > 0) { - for (int channel=0; channel < 4; channel++) { - rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor; - } - } else { - factor = 0; - } -} - // Function for loop trigger void taskGyro(timeUs_t currentTimeUs) { // getTaskDeltaTime() returns delta time frozen at the moment of entering the scheduler. currentTime is frozen at the very same point. @@ -723,8 +687,8 @@ void taskMainPidLoop(timeUs_t currentTimeUs) annexCode(); - if (rxConfig()->rcSmoothing) { - filterRc(isRXDataNew); + if (rxConfig()->rcFilterFrequency) { + rcInterpolationApply(isRXDataNew); } #if defined(USE_NAV) diff --git a/src/main/fc/rc_smoothing.c b/src/main/fc/rc_smoothing.c new file mode 100644 index 00000000000..acfa9c815c3 --- /dev/null +++ b/src/main/fc/rc_smoothing.c @@ -0,0 +1,110 @@ +/* + * This file is part of INAV. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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/. + */ + +#include +#include +#include + +#include "platform.h" + +#include "blackbox/blackbox.h" + +#include "build/debug.h" + +#include "common/maths.h" +#include "common/filter.h" + +#include "drivers/time.h" + +#include "rx/rx.h" + +#include "fc/config.h" +#include "fc/fc_core.h" +#include "fc/rc_controls.h" +#include "fc/rc_smoothing.h" +#include "fc/runtime_config.h" + +#include "flight/mixer.h" + +static biquadFilter_t rcSmoothFilter[4]; +static float rcStickUnfiltered[4]; + +static void rcInterpolationInit(int rcFilterFreqency) +{ + for (int stick = 0; stick < 4; stick++) { + biquadFilterInitLPF(&rcSmoothFilter[stick], rcFilterFreqency, getLooptime()); + } +} + +void rcInterpolationApply(bool isRXDataNew) +{ + static bool initDone = false; + static float initFilterFreqency = 0; + + if (isRXDataNew) { + if (!initDone || (initFilterFreqency != rxConfig()->rcFilterFrequency)) { + rcInterpolationInit(rxConfig()->rcFilterFrequency); + initFilterFreqency = rxConfig()->rcFilterFrequency; + initDone = true; + } + + for (int stick = 0; stick < 4; stick++) { + rcStickUnfiltered[stick] = rcCommand[stick]; + } + } + + // Don't filter if not initialized + if (!initDone) { + return; + } + + for (int stick = 0; stick < 4; stick++) { + rcCommand[stick] = biquadFilterApply(&rcSmoothFilter[stick], rcStickUnfiltered[stick]); + } +} + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/main/fc/rc_smoothing.h b/src/main/fc/rc_smoothing.h new file mode 100644 index 00000000000..4c1ad66bcb6 --- /dev/null +++ b/src/main/fc/rc_smoothing.h @@ -0,0 +1,31 @@ +/* + * This file is part of INAV. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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/. + */ + +#pragma once + +#include +#include +#include + +void rcInterpolationApply(bool isRXDataNew); \ No newline at end of file diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index e7679be9eb8..8a00c98e030 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -354,9 +354,10 @@ groups: field: sbusSyncInterval min: 500 max: 10000 - - name: rc_smoothing - field: rcSmoothing - type: bool + - name: rc_filter_frequency + field: rcFilterFrequency + min: 0 + max: 100 - name: serialrx_provider condition: USE_SERIAL_RX table: serial_rx diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index feefc037be8..cc3f1fa3874 100755 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -100,7 +100,7 @@ timeMs_t rcInvalidPulsPeriod[MAX_SUPPORTED_RC_CHANNEL_COUNT]; rxRuntimeConfig_t rxRuntimeConfig; static uint8_t rcSampleIndex = 0; -PG_REGISTER_WITH_RESET_TEMPLATE(rxConfig_t, rxConfig, PG_RX_CONFIG, 6); +PG_REGISTER_WITH_RESET_TEMPLATE(rxConfig_t, rxConfig, PG_RX_CONFIG, 7); #ifndef RX_SPI_DEFAULT_PROTOCOL #define RX_SPI_DEFAULT_PROTOCOL 0 @@ -130,7 +130,7 @@ PG_RESET_TEMPLATE(rxConfig_t, rxConfig, .rssiMin = RSSI_VISIBLE_VALUE_MIN, .rssiMax = RSSI_VISIBLE_VALUE_MAX, .sbusSyncInterval = SBUS_DEFAULT_INTERFRAME_DELAY_US, - .rcSmoothing = 1, + .rcFilterFrequency = 50, ); void resetAllRxChannelRangeConfigurations(void) diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index e55eca2adb9..7ebb8fa412c 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -129,7 +129,7 @@ typedef struct rxConfig_s { uint16_t maxcheck; // maximum rc end uint16_t rx_min_usec; uint16_t rx_max_usec; - uint8_t rcSmoothing; // Enable/Disable RC filtering + uint8_t rcFilterFrequency; // RC filter cutoff frequency (smoothness vs response sharpness) } rxConfig_t; PG_DECLARE(rxConfig_t, rxConfig); From 8ceb9c7324e3fca4518ff639ec25f753a0a47962 Mon Sep 17 00:00:00 2001 From: "Konstantin Sharlaimov (DigitalEntity)" Date: Mon, 7 Jan 2019 23:29:37 +0100 Subject: [PATCH 2/3] [F3] Disable PCA9685 drifer for targets with <256K Flash (F3) --- src/main/target/common.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/target/common.h b/src/main/target/common.h index 82e40327f11..8a4b7405b99 100755 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -89,6 +89,8 @@ #define USE_DASHBOARD #define DASHBOARD_ARMED_BITMAP #define USE_OLED_UG2864 + +#define USE_PWM_DRIVER_PCA9685 #endif #if (FLASH_SIZE > 128) @@ -125,7 +127,6 @@ #define USE_SERIALRX_CRSF #define USE_PWM_SERVO_DRIVER #define USE_SERIAL_PASSTHROUGH -#define USE_PWM_DRIVER_PCA9685 #define NAV_MAX_WAYPOINTS 60 #define MAX_BOOTLOG_ENTRIES 64 #define USE_RCDEVICE From 0de7f538fb37924bbcdc253b19a98ae652d026d6 Mon Sep 17 00:00:00 2001 From: Konstantin Sharlaimov Date: Tue, 8 Jan 2019 11:12:18 +0100 Subject: [PATCH 3/3] Disable PWM servo driver on SPRF3NEO --- src/main/target/SPRACINGF3NEO/target.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/target/SPRACINGF3NEO/target.h b/src/main/target/SPRACINGF3NEO/target.h index 503ff098b3f..1ca1b823460 100755 --- a/src/main/target/SPRACINGF3NEO/target.h +++ b/src/main/target/SPRACINGF3NEO/target.h @@ -114,6 +114,7 @@ #undef USE_VTX_FFPV #undef USE_VTX_SMARTAUDIO // Disabled due to flash size #undef USE_VTX_TRAMP // Disabled due to flash size +#undef USE_PWM_SERVO_DRIVER // Disabled due to RAM size #define RTC6705_CS_PIN PF4 #define RTC6705_SPI_INSTANCE SPI3