rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-21 11:54:15 +01:00
Code Issues Releases Activity

[OP-724] Add manual control input filtering (useful for camera gimbal)

Browse Source 
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 2012-11-17 12:58:16 +02:00
parent ea1199603b
commit ee333f1569
6 changed files with 217 additions and 95 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
flight/CopterControl/Makefile
View File

@ -77,6 +77,9 @@ USE_ALTITUDE ?= NO
USE_AUTOTUNE ?= YES USE_AUTOTUNE ?= YES
TEST_FAULTS ?= NO TEST_FAULTS ?= NO
# Camera gimbal options
USE_INPUT_LPF ?= YES
# List of optional modules to include # List of optional modules to include
OPTMODULES = OPTMODULES =
ifeq ($(USE_CAMERASTAB), YES) ifeq ($(USE_CAMERASTAB), YES)
@ -459,6 +462,10 @@ ifeq ($(USE_I2C), YES)
CDEFS += -DUSE_I2C CDEFS += -DUSE_I2C
endif endif
ifeq ($(USE_INPUT_LPF), YES)
CDEFS += -DUSE_INPUT_LPF
endif
# Declare all non-optional modules as built-in to force inclusion # Declare all non-optional modules as built-in to force inclusion
CDEFS += ${foreach MOD, ${MODULES}, -DMODULE_$(MOD)_BUILTIN } CDEFS += ${foreach MOD, ${MODULES}, -DMODULE_$(MOD)_BUILTIN }

44
flight/Modules/ManualControl/manualcontrol.c
View File

@ -82,6 +82,11 @@ static xTaskHandle taskHandle;
static ArmState_t armState; static ArmState_t armState;
static portTickType lastSysTime; static portTickType lastSysTime;
#ifdef USE_INPUT_LPF
static portTickType lastSysTimeLPF;
static float inputFiltered[MANUALCONTROLSETTINGS_RESPONSETIME_NUMELEM];
#endif
// Private functions // Private functions
static void updateActuatorDesired(ManualControlCommandData * cmd); static void updateActuatorDesired(ManualControlCommandData * cmd);
static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings); 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 bool validInputRange(int16_t min, int16_t max, uint16_t value);
static void applyDeadband(float *value, float deadband); 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_CHANNELS_PER_GROUP 12
#define RCVR_ACTIVITY_MONITOR_MIN_RANGE 10 #define RCVR_ACTIVITY_MONITOR_MIN_RANGE 10
struct rcvr_activity_fsm { struct rcvr_activity_fsm {
@ -335,7 +344,19 @@ static void manualControlTask(void *parameters)
applyDeadband(&cmd.Pitch, settings.Deadband); applyDeadband(&cmd.Pitch, settings.Deadband);
applyDeadband(&cmd.Yaw, 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 && if(cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != PIOS_RCVR_INVALID &&
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != PIOS_RCVR_NODRIVER && cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != PIOS_RCVR_NODRIVER &&
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != PIOS_RCVR_TIMEOUT) cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != PIOS_RCVR_TIMEOUT)
@ -346,6 +367,9 @@ static void manualControlTask(void *parameters)
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] !=
MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0]; accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0];
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT);
#endif
if(AccessoryDesiredInstSet(0, &accessory) != 0) if(AccessoryDesiredInstSet(0, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
} }
@ -353,6 +377,9 @@ static void manualControlTask(void *parameters)
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] !=
MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1]; accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1];
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT);
#endif
if(AccessoryDesiredInstSet(1, &accessory) != 0) if(AccessoryDesiredInstSet(1, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
} }
@ -360,6 +387,9 @@ static void manualControlTask(void *parameters)
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] !=
MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2]; accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2];
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT);
#endif
if(AccessoryDesiredInstSet(2, &accessory) != 0) if(AccessoryDesiredInstSet(2, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
} }
@ -924,6 +954,20 @@ static void applyDeadband(float *value, float deadband)
*value += 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
/** /**
* @} * @}
* @} * @}

23
ground/openpilotgcs/src/plugins/config/configinputwidget.cpp
View File

@ -77,6 +77,7 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) :
//Generate the rows of buttons in the input channel form GUI //Generate the rows of buttons in the input channel form GUI
unsigned int index=0; unsigned int index=0;
unsigned int indexRT = 0;
foreach (QString name, manualSettingsObj->getField("ChannelNumber")->getElementNames()) foreach (QString name, manualSettingsObj->getField("ChannelNumber")->getElementNames())
{ {
Q_ASSERT(index < ManualControlSettings::CHANNELGROUPS_NUMELEM); Q_ASSERT(index < ManualControlSettings::CHANNELGROUPS_NUMELEM);
@ -88,6 +89,28 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) :
addUAVObjectToWidgetRelation("ManualControlSettings","ChannelMin",inpForm->ui->channelMin,index); addUAVObjectToWidgetRelation("ManualControlSettings","ChannelMin",inpForm->ui->channelMin,index);
addUAVObjectToWidgetRelation("ManualControlSettings","ChannelNeutral",inpForm->ui->channelNeutral,index); addUAVObjectToWidgetRelation("ManualControlSettings","ChannelNeutral",inpForm->ui->channelNeutral,index);
addUAVObjectToWidgetRelation("ManualControlSettings","ChannelMax",inpForm->ui->channelMax,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; ++index;
} }

2
ground/openpilotgcs/src/plugins/config/inputchannelform.cpp
View File

@ -19,12 +19,14 @@ inputChannelForm::inputChannelForm(QWidget *parent,bool showlegend) :
layout()->removeWidget(ui->legend3); layout()->removeWidget(ui->legend3);
layout()->removeWidget(ui->legend4); layout()->removeWidget(ui->legend4);
layout()->removeWidget(ui->legend5); layout()->removeWidget(ui->legend5);
layout()->removeWidget(ui->legend6);
delete ui->legend0; delete ui->legend0;
delete ui->legend1; delete ui->legend1;
delete ui->legend2; delete ui->legend2;
delete ui->legend3; delete ui->legend3;
delete ui->legend4; delete ui->legend4;
delete ui->legend5; delete ui->legend5;
delete ui->legend6;
} }

232
ground/openpilotgcs/src/plugins/config/inputchannelform.ui
View File

@ -236,83 +236,6 @@ font:bold;</string>
</property> </property>
</widget> </widget>
</item> </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"> <item row="1" column="0">
<widget class="QLabel" name="channelName"> <widget class="QLabel" name="channelName">
<property name="sizePolicy"> <property name="sizePolicy">
@ -483,6 +406,144 @@ font:bold;</string>
</property> </property>
</widget> </widget>
</item> </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"> <item row="1" column="8">
<widget class="QLabel" name="neutral"> <widget class="QLabel" name="neutral">
<property name="sizePolicy"> <property name="sizePolicy">
@ -508,22 +569,6 @@ font:bold;</string>
</property> </property>
</widget> </widget>
</item> </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> </layout>
<widget class="QSpinBox" name="channelNumber"> <widget class="QSpinBox" name="channelNumber">
<property name="enabled"> <property name="enabled">
@ -552,7 +597,6 @@ font:bold;</string>
<tabstop>channelMin</tabstop> <tabstop>channelMin</tabstop>
<tabstop>channelNeutral</tabstop> <tabstop>channelNeutral</tabstop>
<tabstop>channelMax</tabstop> <tabstop>channelMax</tabstop>
<tabstop>channelRev</tabstop>
</tabstops> </tabstops>
<resources/> <resources/>
<connections/> <connections/>

2
shared/uavobjectdefinition/manualcontrolsettings.xml
View File

@ -12,6 +12,8 @@
elementnames="Throttle,Roll,Pitch,Yaw,FlightMode,Collective,Accessory0,Accessory1,Accessory2"/> elementnames="Throttle,Roll,Pitch,Yaw,FlightMode,Collective,Accessory0,Accessory1,Accessory2"/>
<field name="ChannelMax" units="us" type="int16" defaultvalue="2000" <field name="ChannelMax" units="us" type="int16" defaultvalue="2000"
elementnames="Throttle,Roll,Pitch,Yaw,FlightMode,Collective,Accessory0,Accessory1,Accessory2"/> 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="Deadband" units="%" type="float" elements="1" defaultvalue="0"/>