From ebcb272da6b549f7a58089ea5c322e18f265b342 Mon Sep 17 00:00:00 2001 From: peabody124 Date: Fri, 26 Nov 2010 15:57:08 +0000 Subject: [PATCH] AHRS: Add a separate parameter for velocity and position variance and remove 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 --- flight/AHRS/ahrs.c | 8 +++---- flight/AHRS/inc/insgps.h | 2 +- flight/AHRS/insgps.c | 6 ++--- flight/OpenPilot/UAVObjects/ahrscalibration.c | 2 ++ flight/OpenPilot/UAVObjects/ahrssettings.c | 1 - .../UAVObjects/inc/ahrscalibration.h | 6 ++++- .../OpenPilot/UAVObjects/inc/ahrssettings.h | 4 +--- .../plugins/uavobjects/ahrscalibration.cpp | 8 +++++++ .../src/plugins/uavobjects/ahrscalibration.h | 6 ++++- .../src/plugins/uavobjects/ahrscalibration.py | 22 ++++++++++++++++++- .../src/plugins/uavobjects/ahrssettings.cpp | 4 ---- ground/src/plugins/uavobjects/ahrssettings.h | 4 +--- ground/src/plugins/uavobjects/ahrssettings.py | 12 +--------- .../uavobjectdefinition/ahrscalibration.xml | 2 ++ .../uavobjectdefinition/ahrssettings.xml | 1 - 15 files changed, 54 insertions(+), 34 deletions(-) diff --git a/flight/AHRS/ahrs.c b/flight/AHRS/ahrs.c index 58112781b..b60401b67 100644 --- a/flight/AHRS/ahrs.c +++ b/flight/AHRS/ahrs.c @@ -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) diff --git a/flight/AHRS/inc/insgps.h b/flight/AHRS/inc/insgps.h index 955b81aad..801a27848 100644 --- a/flight/AHRS/inc/insgps.h +++ b/flight/AHRS/inc/insgps.h @@ -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]); diff --git a/flight/AHRS/insgps.c b/flight/AHRS/insgps.c index b05840b87..b6f5471ef 100644 --- a/flight/AHRS/insgps.c +++ b/flight/AHRS/insgps.c @@ -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 } diff --git a/flight/OpenPilot/UAVObjects/ahrscalibration.c b/flight/OpenPilot/UAVObjects/ahrscalibration.c index fdfa4da0f..6e46cd6e9 100644 --- a/flight/OpenPilot/UAVObjects/ahrscalibration.c +++ b/flight/OpenPilot/UAVObjects/ahrscalibration.c @@ -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); diff --git a/flight/OpenPilot/UAVObjects/ahrssettings.c b/flight/OpenPilot/UAVObjects/ahrssettings.c index 301af53b8..5c5c33783 100644 --- a/flight/OpenPilot/UAVObjects/ahrssettings.c +++ b/flight/OpenPilot/UAVObjects/ahrssettings.c @@ -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; diff --git a/flight/OpenPilot/UAVObjects/inc/ahrscalibration.h b/flight/OpenPilot/UAVObjects/inc/ahrscalibration.h index d62b6d0aa..ed5a105ca 100644 --- a/flight/OpenPilot/UAVObjects/inc/ahrscalibration.h +++ b/flight/OpenPilot/UAVObjects/inc/ahrscalibration.h @@ -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 diff --git a/flight/OpenPilot/UAVObjects/inc/ahrssettings.h b/flight/OpenPilot/UAVObjects/inc/ahrssettings.h index e73af9e0f..7143c5800 100644 --- a/flight/OpenPilot/UAVObjects/inc/ahrssettings.h +++ b/flight/OpenPilot/UAVObjects/inc/ahrssettings.h @@ -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 diff --git a/ground/src/plugins/uavobjects/ahrscalibration.cpp b/ground/src/plugins/uavobjects/ahrscalibration.cpp index 3038797bf..6e1f2d624 100644 --- a/ground/src/plugins/uavobjects/ahrscalibration.cpp +++ b/ground/src/plugins/uavobjects/ahrscalibration.cpp @@ -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; } diff --git a/ground/src/plugins/uavobjects/ahrscalibration.h b/ground/src/plugins/uavobjects/ahrscalibration.h index 023232321..105bf090b 100644 --- a/ground/src/plugins/uavobjects/ahrscalibration.h +++ b/ground/src/plugins/uavobjects/ahrscalibration.h @@ -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; diff --git a/ground/src/plugins/uavobjects/ahrscalibration.py b/ground/src/plugins/uavobjects/ahrscalibration.py index ee420b8ba..bc92ea0f2 100644 --- a/ground/src/plugins/uavobjects/ahrscalibration.py +++ b/ground/src/plugins/uavobjects/ahrscalibration.py @@ -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 diff --git a/ground/src/plugins/uavobjects/ahrssettings.cpp b/ground/src/plugins/uavobjects/ahrssettings.cpp index e6c629600..eae5c6e8f 100644 --- a/ground/src/plugins/uavobjects/ahrssettings.cpp +++ b/ground/src/plugins/uavobjects/ahrssettings.cpp @@ -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; diff --git a/ground/src/plugins/uavobjects/ahrssettings.h b/ground/src/plugins/uavobjects/ahrssettings.h index 2d723aeda..2ba86f6d9 100644 --- a/ground/src/plugins/uavobjects/ahrssettings.h +++ b/ground/src/plugins/uavobjects/ahrssettings.h @@ -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; diff --git a/ground/src/plugins/uavobjects/ahrssettings.py b/ground/src/plugins/uavobjects/ahrssettings.py index 202ef48a1..b2b020bf9 100644 --- a/ground/src/plugins/uavobjects/ahrssettings.py +++ b/ground/src/plugins/uavobjects/ahrssettings.py @@ -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 diff --git a/ground/src/shared/uavobjectdefinition/ahrscalibration.xml b/ground/src/shared/uavobjectdefinition/ahrscalibration.xml index 77b7b9b87..e6f2af100 100644 --- a/ground/src/shared/uavobjectdefinition/ahrscalibration.xml +++ b/ground/src/shared/uavobjectdefinition/ahrscalibration.xml @@ -11,6 +11,8 @@ + + diff --git a/ground/src/shared/uavobjectdefinition/ahrssettings.xml b/ground/src/shared/uavobjectdefinition/ahrssettings.xml index 8980ff89e..9d278bdb6 100644 --- a/ground/src/shared/uavobjectdefinition/ahrssettings.xml +++ b/ground/src/shared/uavobjectdefinition/ahrssettings.xml @@ -4,7 +4,6 @@ -