From ee333f1569f5ee8a943aea2b0bb697a39209c3a4 Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Sat, 17 Nov 2012 12:58:16 +0200 Subject: [PATCH] [OP-724] Add manual control input filtering (useful for camera gimbal) This is a partial rework of Cossacs' camera gimbal software. This patch adds LPF to some of manual control inputs. Mostly useful are accessory channels (for camera gimbal control) and yaw channel (for smooth filming). The code may be used for stand-alone CC[3D]-based gimbal software, but also should work for complete FC+camera system. --- flight/CopterControl/Makefile | 7 + flight/Modules/ManualControl/manualcontrol.c | 44 ++++ .../src/plugins/config/configinputwidget.cpp | 23 ++ .../src/plugins/config/inputchannelform.cpp | 2 + .../src/plugins/config/inputchannelform.ui | 232 +++++++++++------- .../manualcontrolsettings.xml | 4 +- 6 files changed, 217 insertions(+), 95 deletions(-) diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index 128968585..5db7dcf0a 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -77,6 +77,9 @@ USE_ALTITUDE ?= NO USE_AUTOTUNE ?= YES TEST_FAULTS ?= NO +# Camera gimbal options +USE_INPUT_LPF ?= YES + # List of optional modules to include OPTMODULES = ifeq ($(USE_CAMERASTAB), YES) @@ -459,6 +462,10 @@ ifeq ($(USE_I2C), YES) CDEFS += -DUSE_I2C endif +ifeq ($(USE_INPUT_LPF), YES) +CDEFS += -DUSE_INPUT_LPF +endif + # Declare all non-optional modules as built-in to force inclusion CDEFS += ${foreach MOD, ${MODULES}, -DMODULE_$(MOD)_BUILTIN } diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index 2c7a68575..f806c14d5 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -82,6 +82,11 @@ static xTaskHandle taskHandle; static ArmState_t armState; static portTickType lastSysTime; +#ifdef USE_INPUT_LPF +static portTickType lastSysTimeLPF; +static float inputFiltered[MANUALCONTROLSETTINGS_RESPONSETIME_NUMELEM]; +#endif + // Private functions static void updateActuatorDesired(ManualControlCommandData * cmd); static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings); @@ -97,6 +102,10 @@ static bool okToArm(void); static bool validInputRange(int16_t min, int16_t max, uint16_t value); static void applyDeadband(float *value, float deadband); +#ifdef USE_INPUT_LPF +static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT); +#endif + #define RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP 12 #define RCVR_ACTIVITY_MONITOR_MIN_RANGE 10 struct rcvr_activity_fsm { @@ -335,7 +344,19 @@ static void manualControlTask(void *parameters) applyDeadband(&cmd.Pitch, settings.Deadband); applyDeadband(&cmd.Yaw, settings.Deadband); } +#ifdef USE_INPUT_LPF + // Apply Low Pass Filter to input channels, time delta between calls in ms + portTickType thisSysTime = xTaskGetTickCount(); + float dT = (thisSysTime > lastSysTimeLPF) ? + (float)((thisSysTime - lastSysTimeLPF) * portTICK_RATE_MS) : + (float)UPDATE_PERIOD_MS; + lastSysTimeLPF = thisSysTime; + applyLPF(&cmd.Throttle, MANUALCONTROLSETTINGS_RESPONSETIME_THROTTLE, &settings, dT); + applyLPF(&cmd.Roll, MANUALCONTROLSETTINGS_RESPONSETIME_ROLL, &settings, dT); + applyLPF(&cmd.Pitch, MANUALCONTROLSETTINGS_RESPONSETIME_PITCH, &settings, dT); + applyLPF(&cmd.Yaw, MANUALCONTROLSETTINGS_RESPONSETIME_YAW, &settings, dT); +#endif // USE_INPUT_LPF if(cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != PIOS_RCVR_INVALID && cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != PIOS_RCVR_NODRIVER && cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != PIOS_RCVR_TIMEOUT) @@ -346,6 +367,9 @@ static void manualControlTask(void *parameters) if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0]; +#ifdef USE_INPUT_LPF + applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT); +#endif if(AccessoryDesiredInstSet(0, &accessory) != 0) AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); } @@ -353,6 +377,9 @@ static void manualControlTask(void *parameters) if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1]; +#ifdef USE_INPUT_LPF + applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT); +#endif if(AccessoryDesiredInstSet(1, &accessory) != 0) AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); } @@ -360,6 +387,9 @@ static void manualControlTask(void *parameters) if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2]; +#ifdef USE_INPUT_LPF + applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT); +#endif if(AccessoryDesiredInstSet(2, &accessory) != 0) AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); } @@ -924,6 +954,20 @@ static void applyDeadband(float *value, float deadband) *value += deadband; } +#ifdef USE_INPUT_LPF +/** + * @brief Apply Low Pass Filter to Throttle/Roll/Pitch/Yaw or Accessory channel + */ +static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT) +{ + if (settings->ResponseTime[channel]) { + float rt = (float)settings->ResponseTime[channel]; + inputFiltered[channel] = ((rt * inputFiltered[channel]) + (dT * (*value))) / (rt + dT); + *value = inputFiltered[channel]; + } +} +#endif // USE_INPUT_LPF + /** * @} * @} diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp index 44bc92bc4..26c384c58 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp @@ -77,6 +77,7 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : //Generate the rows of buttons in the input channel form GUI unsigned int index=0; + unsigned int indexRT = 0; foreach (QString name, manualSettingsObj->getField("ChannelNumber")->getElementNames()) { Q_ASSERT(index < ManualControlSettings::CHANNELGROUPS_NUMELEM); @@ -88,6 +89,28 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : addUAVObjectToWidgetRelation("ManualControlSettings","ChannelMin",inpForm->ui->channelMin,index); addUAVObjectToWidgetRelation("ManualControlSettings","ChannelNeutral",inpForm->ui->channelNeutral,index); addUAVObjectToWidgetRelation("ManualControlSettings","ChannelMax",inpForm->ui->channelMax,index); + + // Input filter response time fields supported for some channels only + switch (index) { + case ManualControlSettings::CHANNELGROUPS_THROTTLE: + case ManualControlSettings::CHANNELGROUPS_ROLL: + case ManualControlSettings::CHANNELGROUPS_PITCH: + case ManualControlSettings::CHANNELGROUPS_YAW: + case ManualControlSettings::CHANNELGROUPS_ACCESSORY0: + case ManualControlSettings::CHANNELGROUPS_ACCESSORY1: + case ManualControlSettings::CHANNELGROUPS_ACCESSORY2: + addUAVObjectToWidgetRelation("ManualControlSettings", "ResponseTime", inpForm->ui->channelResponseTime, indexRT); + ++indexRT; + break; + case ManualControlSettings::CHANNELGROUPS_FLIGHTMODE: + case ManualControlSettings::CHANNELGROUPS_COLLECTIVE: + inpForm->ui->channelResponseTime->setEnabled(false); + break; + default: + Q_ASSERT(0); + break; + } + ++index; } diff --git a/ground/openpilotgcs/src/plugins/config/inputchannelform.cpp b/ground/openpilotgcs/src/plugins/config/inputchannelform.cpp index bd2d5830e..df27b6a14 100755 --- a/ground/openpilotgcs/src/plugins/config/inputchannelform.cpp +++ b/ground/openpilotgcs/src/plugins/config/inputchannelform.cpp @@ -19,12 +19,14 @@ inputChannelForm::inputChannelForm(QWidget *parent,bool showlegend) : layout()->removeWidget(ui->legend3); layout()->removeWidget(ui->legend4); layout()->removeWidget(ui->legend5); + layout()->removeWidget(ui->legend6); delete ui->legend0; delete ui->legend1; delete ui->legend2; delete ui->legend3; delete ui->legend4; delete ui->legend5; + delete ui->legend6; } diff --git a/ground/openpilotgcs/src/plugins/config/inputchannelform.ui b/ground/openpilotgcs/src/plugins/config/inputchannelform.ui index 968e12c14..7a111d080 100755 --- a/ground/openpilotgcs/src/plugins/config/inputchannelform.ui +++ b/ground/openpilotgcs/src/plugins/config/inputchannelform.ui @@ -236,83 +236,6 @@ font:bold; - - - - true - - - - 0 - 0 - - - - - 0 - 20 - - - - - 75 - false - true - - - - background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); -color: rgb(255, 255, 255); -border-radius: 5; -margin:1px; -font:bold; - - - QFrame::StyledPanel - - - 1 - - - Max - - - Qt::AlignCenter - - - - - - - Qt::Horizontal - - - QSizePolicy::Fixed - - - - 48 - 10 - - - - - - - - Qt::Horizontal - - - QSizePolicy::Fixed - - - - 42 - 10 - - - - @@ -483,6 +406,144 @@ font:bold; + + + + true + + + + 0 + 0 + + + + + 0 + 20 + + + + + 75 + false + true + + + + background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); +color: rgb(255, 255, 255); +border-radius: 5; +margin:1px; +font:bold; + + + QFrame::StyledPanel + + + 1 + + + Max + + + Qt::AlignCenter + + + + + + + false + + + + 0 + 20 + + + + Rev + + + + + + + true + + + + 0 + 0 + + + + + 30 + 25 + + + + Optional input filter response time. + +Range: 0-999ms, 0 disables filter (default). + +Warning: this is an expert mode feature, mostly used for aerial video +camera control (airframe yaw and camera gimbal accessory channels). +Too high values for main controls can cause undesirable effects and +even lead to crash. Use with caution. + + + false + + + true + + + QAbstractSpinBox::NoButtons + + + 999 + + + + + + + + 0 + 0 + + + + + 30 + 26 + + + + + 75 + false + true + + + + background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); +color: rgb(255, 255, 255); +border-radius: 5; +margin:5px; +font:bold; + + + RT + + + Qt::AlignCenter + + + @@ -508,22 +569,6 @@ font:bold; - - - - false - - - - 0 - 20 - - - - Rev - - - @@ -552,7 +597,6 @@ font:bold; channelMin channelNeutral channelMax - channelRev diff --git a/shared/uavobjectdefinition/manualcontrolsettings.xml b/shared/uavobjectdefinition/manualcontrolsettings.xml index 396dcfea1..23930f1d1 100644 --- a/shared/uavobjectdefinition/manualcontrolsettings.xml +++ b/shared/uavobjectdefinition/manualcontrolsettings.xml @@ -12,7 +12,9 @@ elementnames="Throttle,Roll,Pitch,Yaw,FlightMode,Collective,Accessory0,Accessory1,Accessory2"/> - + +