rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

AHRS: Paramaterize the variance used for the indoor algorithm.

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1898 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-10-06 14:26:40 +00:00 committed by peabody124
parent 86338404ed
commit 66bf9d53d4
8 changed files with 27 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
flight/AHRS/ahrs.c
View File

@ -392,7 +392,9 @@ for all data to be up to date before doing anything*/
mag_data.updated = 0;
} else {
// Indoors, update with zero position and velocity and high covariance
INSSetPosVelVar(0.1);
AHRSSettingsData settings;
AHRSSettingsGet(&settings);
INSSetPosVelVar(settings.IndoorVelocityVariance);
vel[0] = 0;
vel[1] = 0;
vel[2] = 0;

2
flight/AHRS/insgps.c
View File

@ -88,7 +88,7 @@ void INSGPSInit() //pretty much just a place holder for now
Q[0] = Q[1] = Q[2] = 50e-8; // gyro noise variance (rad/s)^2
Q[3] = Q[4] = Q[5] = 0.01; // accelerometer noise variance (m/s^2)^2
Q[6] = Q[7] = Q[8] = 2e-7; // gyro bias random walk variance (rad/s^2)^2
Q[6] = Q[7] = Q[8] = 2e-9; // gyro bias random walk variance (rad/s^2)^2
R[0] = R[1] = 0.004; // High freq GPS horizontal position noise variance (m^2)
R[2] = 0.036; // High freq GPS vertical position noise variance (m^2)

1
flight/OpenPilot/UAVObjects/ahrssettings.c
View File

@ -83,6 +83,7 @@ static void setDefaults(UAVObjHandle obj, uint16_t instId)
data.Algorithm = 1;
data.Downsampling = 20;
data.UpdatePeriod = 1;
data.IndoorVelocityVariance = 0.04;
data.YawBias = 0;
data.PitchBias = 0;
data.RollBias = 0;

4
flight/OpenPilot/UAVObjects/inc/ahrssettings.h
View File

@ -41,7 +41,7 @@
#define AHRSSETTINGS_H
// Object constants
#define AHRSSETTINGS_OBJID 1456050280U
#define AHRSSETTINGS_OBJID 3427867870U
#define AHRSSETTINGS_NAME "AHRSSettings"
#define AHRSSETTINGS_METANAME "AHRSSettingsMeta"
#define AHRSSETTINGS_ISSINGLEINST 1
@ -74,6 +74,7 @@ typedef struct {
uint8_t Algorithm;
uint8_t Downsampling;
uint8_t UpdatePeriod;
float IndoorVelocityVariance;
float YawBias;
float PitchBias;
float RollBias;
@ -86,6 +87,7 @@ typedef struct {
typedef enum { AHRSSETTINGS_ALGORITHM_SIMPLE=0, AHRSSETTINGS_ALGORITHM_INSGPS_INDOOR_NOMAG=1, AHRSSETTINGS_ALGORITHM_INSGPS_INDOOR=2, AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR=3 } AHRSSettingsAlgorithmOptions;
// Field Downsampling information
// Field UpdatePeriod information
// Field IndoorVelocityVariance information
// Field YawBias information
// Field PitchBias information
// Field RollBias information

4
ground/src/plugins/uavobjects/ahrssettings.cpp
View File

@ -56,6 +56,9 @@ AHRSSettings::AHRSSettings(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTINGS, NAM
QStringList UpdatePeriodElemNames;
UpdatePeriodElemNames.append("0");
fields.append( new UAVObjectField(QString("UpdatePeriod"), QString("ms"), UAVObjectField::UINT8, UpdatePeriodElemNames, QStringList()) );
QStringList IndoorVelocityVarianceElemNames;
IndoorVelocityVarianceElemNames.append("0");
fields.append( new UAVObjectField(QString("IndoorVelocityVariance"), QString("m^2"), UAVObjectField::FLOAT32, IndoorVelocityVarianceElemNames, QStringList()) );
QStringList YawBiasElemNames;
YawBiasElemNames.append("0");
fields.append( new UAVObjectField(QString("YawBias"), QString(""), UAVObjectField::FLOAT32, YawBiasElemNames, QStringList()) );
@ -101,6 +104,7 @@ void AHRSSettings::setDefaultFieldValues()
data.Algorithm = 1;
data.Downsampling = 20;
data.UpdatePeriod = 1;
data.IndoorVelocityVariance = 0.04;
data.YawBias = 0;
data.PitchBias = 0;
data.RollBias = 0;

4
ground/src/plugins/uavobjects/ahrssettings.h
View File

@ -46,6 +46,7 @@ public:
quint8 Algorithm;
quint8 Downsampling;
quint8 UpdatePeriod;
float IndoorVelocityVariance;
float YawBias;
float PitchBias;
float RollBias;
@ -58,13 +59,14 @@ public:
typedef enum { ALGORITHM_SIMPLE=0, ALGORITHM_INSGPS_INDOOR_NOMAG=1, ALGORITHM_INSGPS_INDOOR=2, ALGORITHM_INSGPS_OUTDOOR=3 } AlgorithmOptions;
// Field Downsampling information
// Field UpdatePeriod information
// Field IndoorVelocityVariance information
// Field YawBias information
// Field PitchBias information
// Field RollBias information
// Constants
static const quint32 OBJID = 1456050280U;
static const quint32 OBJID = 3427867870U;
static const QString NAME;
static const bool ISSINGLEINST = 1;
static const bool ISSETTINGS = 1;

12
ground/src/plugins/uavobjects/ahrssettings.py
View File

@ -71,6 +71,16 @@ _fields = [ \
{
}
),
uavobject.UAVObjectField(
'IndoorVelocityVariance',
'f',
1,
[
'0',
],
{
}
),
uavobject.UAVObjectField(
'YawBias',
'f',
@ -106,7 +116,7 @@ _fields = [ \
class AHRSSettings(uavobject.UAVObject):
## Object constants
OBJID = 1456050280
OBJID = 3427867870
NAME = "AHRSSettings"
METANAME = "AHRSSettingsMeta"
ISSINGLEINST = 1

1
ground/src/shared/uavobjectdefinition/ahrssettings.xml
View File

@ -4,6 +4,7 @@
<field name="Algorithm" units="" type="enum" elements="1" options="SIMPLE,INSGPS_INDOOR_NOMAG,INSGPS_INDOOR,INSGPS_OUTDOOR" defaultvalue="INSGPS_INDOOR_NOMAG"/>
<field name="Downsampling" units="" type="uint8" elements="1" defaultvalue="20"/>
<field name="UpdatePeriod" units="ms" type="uint8" elements="1" defaultvalue="1"/>
<field name="IndoorVelocityVariance" units="m^2" type="float" elements="1" defaultvalue="0.04"/>
<field name="YawBias" units="" type="float" elements="1" defaultvalue="0"/>
<field name="PitchBias" units="" type="float" elements="1" defaultvalue="0"/>
<field name="RollBias" units="" type="float" elements="1" defaultvalue="0"/>