mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-20 10:54:14 +01:00
[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.
This commit is contained in:
Oleg Semyonov
parent
ea1199603b
commit
ee333f1569
@ -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 }
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -236,83 +236,6 @@ font:bold;</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="7">
|
||||
<widget class="QLabel" name="legend5">
|
||||
<property name="enabled">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<property name="sizePolicy">
|
||||
<sizepolicy hsizetype="Minimum" vsizetype="Fixed">
|
||||
<horstretch>0</horstretch>
|
||||
<verstretch>0</verstretch>
|
||||
</sizepolicy>
|
||||
</property>
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>0</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="font">
|
||||
<font>
|
||||
<weight>75</weight>
|
||||
<italic>false</italic>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="styleSheet">
|
||||
<string notr="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;</string>
|
||||
</property>
|
||||
<property name="frameShape">
|
||||
<enum>QFrame::StyledPanel</enum>
|
||||
</property>
|
||||
<property name="lineWidth">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Max</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="8">
|
||||
<spacer name="horizontalSpacer_3">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="sizeType">
|
||||
<enum>QSizePolicy::Fixed</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>48</width>
|
||||
<height>10</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item row="0" column="9">
|
||||
<spacer name="horizontalSpacer_4">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="sizeType">
|
||||
<enum>QSizePolicy::Fixed</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>42</width>
|
||||
<height>10</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item row="1" column="0">
|
||||
<widget class="QLabel" name="channelName">
|
||||
<property name="sizePolicy">
|
||||
@ -483,6 +406,144 @@ font:bold;</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="7">
|
||||
<widget class="QLabel" name="legend5">
|
||||
<property name="enabled">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<property name="sizePolicy">
|
||||
<sizepolicy hsizetype="Minimum" vsizetype="Fixed">
|
||||
<horstretch>0</horstretch>
|
||||
<verstretch>0</verstretch>
|
||||
</sizepolicy>
|
||||
</property>
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>0</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="font">
|
||||
<font>
|
||||
<weight>75</weight>
|
||||
<italic>false</italic>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="styleSheet">
|
||||
<string notr="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;</string>
|
||||
</property>
|
||||
<property name="frameShape">
|
||||
<enum>QFrame::StyledPanel</enum>
|
||||
</property>
|
||||
<property name="lineWidth">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Max</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="9">
|
||||
<widget class="QCheckBox" name="channelRev">
|
||||
<property name="enabled">
|
||||
<bool>false</bool>
|
||||
</property>
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>0</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Rev</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="10">
|
||||
<widget class="QSpinBox" name="channelResponseTime">
|
||||
<property name="enabled">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<property name="sizePolicy">
|
||||
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
|
||||
<horstretch>0</horstretch>
|
||||
<verstretch>0</verstretch>
|
||||
</sizepolicy>
|
||||
</property>
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>30</width>
|
||||
<height>25</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="toolTip">
|
||||
<string>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.</string>
|
||||
</property>
|
||||
<property name="wrapping">
|
||||
<bool>false</bool>
|
||||
</property>
|
||||
<property name="frame">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<property name="buttonSymbols">
|
||||
<enum>QAbstractSpinBox::NoButtons</enum>
|
||||
</property>
|
||||
<property name="maximum">
|
||||
<number>999</number>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="10">
|
||||
<widget class="QLabel" name="legend6">
|
||||
<property name="sizePolicy">
|
||||
<sizepolicy hsizetype="Minimum" vsizetype="Fixed">
|
||||
<horstretch>0</horstretch>
|
||||
<verstretch>0</verstretch>
|
||||
</sizepolicy>
|
||||
</property>
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>30</width>
|
||||
<height>26</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="font">
|
||||
<font>
|
||||
<weight>75</weight>
|
||||
<italic>false</italic>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="styleSheet">
|
||||
<string notr="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;</string>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>RT</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="8">
|
||||
<widget class="QLabel" name="neutral">
|
||||
<property name="sizePolicy">
|
||||
@ -508,22 +569,6 @@ font:bold;</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="9">
|
||||
<widget class="QCheckBox" name="channelRev">
|
||||
<property name="enabled">
|
||||
<bool>false</bool>
|
||||
</property>
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>0</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Rev</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
<widget class="QSpinBox" name="channelNumber">
|
||||
<property name="enabled">
|
||||
@ -552,7 +597,6 @@ font:bold;</string>
|
||||
<tabstop>channelMin</tabstop>
|
||||
<tabstop>channelNeutral</tabstop>
|
||||
<tabstop>channelMax</tabstop>
|
||||
<tabstop>channelRev</tabstop>
|
||||
</tabstops>
|
||||
<resources/>
|
||||
<connections/>
|
||||
|
||||
@ -12,7 +12,9 @@
|
||||
elementnames="Throttle,Roll,Pitch,Yaw,FlightMode,Collective,Accessory0,Accessory1,Accessory2"/>
|
||||
<field name="ChannelMax" units="us" type="int16" defaultvalue="2000"
|
||||
elementnames="Throttle,Roll,Pitch,Yaw,FlightMode,Collective,Accessory0,Accessory1,Accessory2"/>
|
||||
|
||||
<field name="ResponseTime" units="ms" type="uint16" defaultvalue="0"
|
||||
elementnames="Throttle,Roll,Pitch,Yaw,Accessory0,Accessory1,Accessory2"/>
|
||||
|
||||
<field name="Deadband" units="%" type="float" elements="1" defaultvalue="0"/>
|
||||
|
||||
<field name="Arming" units="" type="enum" elements="1" options="Always Disarmed,Always Armed,Roll Left,Roll Right,Pitch Forward,Pitch Aft,Yaw Left,Yaw Right" defaultvalue="Always Disarmed"/>
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user