rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00
Code Issues Releases Activity

AHRS: Add a separate parameter for velocity and position variance and remove

Browse Source 
indoor from the name since it's used for both indoor and outdoor mode now.
Increased default value to 0.4 to better reflect the GPS accuracy.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2156 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-11-26 15:57:08 +00:00 committed by peabody124
parent ac734d5f65
commit ebcb272da6
15 changed files with 54 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
flight/AHRS/ahrs.c
View File

@ -911,6 +911,9 @@ void calibration_callback(AhrsObjHandle obj)
calibrate_sensors();
send_calibration();
}
INSSetPosVelVar(cal.pos_var, cal.vel_var);
}
void gps_callback(AhrsObjHandle obj)
@ -973,10 +976,7 @@ void settings_callback(AhrsObjHandle obj)
fir_coeffs[i] = 1;
fir_coeffs[adc_oversampling] = adc_oversampling;
}
INSSetPosVelVar(settings.IndoorVelocityVariance);
}
}
void homelocation_callback(AhrsObjHandle obj)

2
flight/AHRS/inc/insgps.h
View File

@ -58,7 +58,7 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt,
void INSResetP(float PDiag[13]);
void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3]);
void INSSetPosVelVar(float PosVar);
void INSSetPosVelVar(float PosVar, float VelVar);
void INSSetGyroBias(float gyro_bias[3]);
void INSSetAccelVar(float accel_var[3]);
void INSSetGyroVar(float gyro_var[3]);

6
flight/AHRS/insgps.c
View File

@ -153,13 +153,13 @@ void INSPosVelReset(float pos[3], float vel[3])
X[5] = vel[2];
}
void INSSetPosVelVar(float PosVar)
void INSSetPosVelVar(float PosVar, float VelVar)
{
R[0] = PosVar;
R[1] = PosVar;
R[2] = PosVar;
R[3] = PosVar;
R[4] = PosVar;
R[3] = VelVar;
R[4] = VelVar;
// R[5] = PosVar; // Don't change vertical velocity, not measured
}

2
flight/OpenPilot/UAVObjects/ahrscalibration.c
View File

@ -108,6 +108,8 @@ static void setDefaults(UAVObjHandle obj, uint16_t instId)
data.mag_var[0] = 5e-05;
data.mag_var[1] = 5e-05;
data.mag_var[2] = 5e-05;
data.vel_var = 0.4;
data.pos_var = 0.4;
UAVObjSetInstanceData(obj, instId, &data);

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

@ -83,7 +83,6 @@ 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;

6
flight/OpenPilot/UAVObjects/inc/ahrscalibration.h
View File

@ -41,7 +41,7 @@
#define AHRSCALIBRATION_H
// Object constants
#define AHRSCALIBRATION_OBJID 1408636690U
#define AHRSCALIBRATION_OBJID 3761202980U
#define AHRSCALIBRATION_NAME "AHRSCalibration"
#define AHRSCALIBRATION_METANAME "AHRSCalibrationMeta"
#define AHRSCALIBRATION_ISSINGLEINST 1
@ -81,6 +81,8 @@ typedef struct {
float mag_bias[3];
float mag_scale[3];
float mag_var[3];
float vel_var;
float pos_var;
} __attribute__((packed)) AHRSCalibrationData;
@ -133,6 +135,8 @@ typedef enum { AHRSCALIBRATION_MAG_SCALE_X=0, AHRSCALIBRATION_MAG_SCALE_Y=1, AHR
typedef enum { AHRSCALIBRATION_MAG_VAR_X=0, AHRSCALIBRATION_MAG_VAR_Y=1, AHRSCALIBRATION_MAG_VAR_Z=2 } AHRSCalibrationmag_varElem;
/* Number of elements for field mag_var */
#define AHRSCALIBRATION_MAG_VAR_NUMELEM 3
// Field vel_var information
// Field pos_var information
// Generic interface functions

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

@ -41,7 +41,7 @@
#define AHRSSETTINGS_H
// Object constants
#define AHRSSETTINGS_OBJID 3427867870U
#define AHRSSETTINGS_OBJID 1456050280U
#define AHRSSETTINGS_NAME "AHRSSettings"
#define AHRSSETTINGS_METANAME "AHRSSettingsMeta"
#define AHRSSETTINGS_ISSINGLEINST 1
@ -74,7 +74,6 @@ typedef struct {
uint8_t Algorithm;
uint8_t Downsampling;
uint8_t UpdatePeriod;
float IndoorVelocityVariance;
float YawBias;
float PitchBias;
float RollBias;
@ -87,7 +86,6 @@ 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

8
ground/src/plugins/uavobjects/ahrscalibration.cpp
View File

@ -93,6 +93,12 @@ AHRSCalibration::AHRSCalibration(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTING
mag_varElemNames.append("Y");
mag_varElemNames.append("Z");
fields.append( new UAVObjectField(QString("mag_var"), QString("mGau^s"), UAVObjectField::FLOAT32, mag_varElemNames, QStringList()) );
QStringList vel_varElemNames;
vel_varElemNames.append("0");
fields.append( new UAVObjectField(QString("vel_var"), QString("(m/s)^2"), UAVObjectField::FLOAT32, vel_varElemNames, QStringList()) );
QStringList pos_varElemNames;
pos_varElemNames.append("0");
fields.append( new UAVObjectField(QString("pos_var"), QString("m^2"), UAVObjectField::FLOAT32, pos_varElemNames, QStringList()) );
// Initialize object
initializeFields(fields, (quint8*)&data, NUMBYTES);
@ -154,6 +160,8 @@ void AHRSCalibration::setDefaultFieldValues()
data.mag_var[0] = 5e-05;
data.mag_var[1] = 5e-05;
data.mag_var[2] = 5e-05;
data.vel_var = 0.4;
data.pos_var = 0.4;
}

6
ground/src/plugins/uavobjects/ahrscalibration.h
View File

@ -53,6 +53,8 @@ public:
float mag_bias[3];
float mag_scale[3];
float mag_var[3];
float vel_var;
float pos_var;
} __attribute__((packed)) DataFields;
@ -105,10 +107,12 @@ public:
typedef enum { MAG_VAR_X=0, MAG_VAR_Y=1, MAG_VAR_Z=2 } mag_varElem;
/* Number of elements for field mag_var */
static const quint32 MAG_VAR_NUMELEM = 3;
// Field vel_var information
// Field pos_var information
// Constants
static const quint32 OBJID = 1408636690U;
static const quint32 OBJID = 3761202980U;
static const QString NAME;
static const bool ISSINGLEINST = 1;
static const bool ISSETTINGS = 1;

22
ground/src/plugins/uavobjects/ahrscalibration.py
View File

@ -157,12 +157,32 @@ _fields = [ \
{
}
),
uavobject.UAVObjectField(
'vel_var',
'f',
1,
[
'0',
],
{
}
),
uavobject.UAVObjectField(
'pos_var',
'f',
1,
[
'0',
],
{
}
),
]
class AHRSCalibration(uavobject.UAVObject):
## Object constants
OBJID = 1408636690
OBJID = 3761202980
NAME = "AHRSCalibration"
METANAME = "AHRSCalibrationMeta"
ISSINGLEINST = 1

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

@ -56,9 +56,6 @@ 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()) );
@ -104,7 +101,6 @@ 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,7 +46,6 @@ public:
quint8 Algorithm;
quint8 Downsampling;
quint8 UpdatePeriod;
float IndoorVelocityVariance;
float YawBias;
float PitchBias;
float RollBias;
@ -59,14 +58,13 @@ 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 = 3427867870U;
static const quint32 OBJID = 1456050280U;
static const QString NAME;
static const bool ISSINGLEINST = 1;
static const bool ISSETTINGS = 1;

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

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

2
ground/src/shared/uavobjectdefinition/ahrscalibration.xml
View File

@ -11,6 +11,8 @@
<field name="mag_bias" units="mGau" type="float" elementnames="X,Y,Z" defaultvalue="0"/>
<field name="mag_scale" units="mGau" type="float" elementnames="X,Y,Z" defaultvalue="1"/>
<field name="mag_var" units="mGau^s" type="float" elementnames="X,Y,Z" defaultvalue="5e-5"/>
<field name="vel_var" units="(m/s)^2" type="float" elements="1" defaultvalue="0.4"/>
<field name="pos_var" units="m^2" type="float" elements="1" defaultvalue="0.4"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>

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

@ -4,7 +4,6 @@
<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"/>