diff --git a/flight/AHRS/ahrs.c b/flight/AHRS/ahrs.c
index 8c4682bf2..ab4671715 100644
--- a/flight/AHRS/ahrs.c
+++ b/flight/AHRS/ahrs.c
@@ -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;
diff --git a/flight/AHRS/insgps.c b/flight/AHRS/insgps.c
index f952d0510..1bcefa104 100644
--- a/flight/AHRS/insgps.c
+++ b/flight/AHRS/insgps.c
@@ -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)
diff --git a/flight/OpenPilot/UAVObjects/ahrssettings.c b/flight/OpenPilot/UAVObjects/ahrssettings.c
index 5c5c33783..301af53b8 100644
--- a/flight/OpenPilot/UAVObjects/ahrssettings.c
+++ b/flight/OpenPilot/UAVObjects/ahrssettings.c
@@ -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;
diff --git a/flight/OpenPilot/UAVObjects/inc/ahrssettings.h b/flight/OpenPilot/UAVObjects/inc/ahrssettings.h
index 7143c5800..e73af9e0f 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 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
diff --git a/ground/src/plugins/uavobjects/ahrssettings.cpp b/ground/src/plugins/uavobjects/ahrssettings.cpp
index eae5c6e8f..e6c629600 100644
--- a/ground/src/plugins/uavobjects/ahrssettings.cpp
+++ b/ground/src/plugins/uavobjects/ahrssettings.cpp
@@ -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;
diff --git a/ground/src/plugins/uavobjects/ahrssettings.h b/ground/src/plugins/uavobjects/ahrssettings.h
index 2ba86f6d9..2d723aeda 100644
--- a/ground/src/plugins/uavobjects/ahrssettings.h
+++ b/ground/src/plugins/uavobjects/ahrssettings.h
@@ -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;
diff --git a/ground/src/plugins/uavobjects/ahrssettings.py b/ground/src/plugins/uavobjects/ahrssettings.py
index b2b020bf9..202ef48a1 100644
--- a/ground/src/plugins/uavobjects/ahrssettings.py
+++ b/ground/src/plugins/uavobjects/ahrssettings.py
@@ -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
diff --git a/ground/src/shared/uavobjectdefinition/ahrssettings.xml b/ground/src/shared/uavobjectdefinition/ahrssettings.xml
index 9d278bdb6..8980ff89e 100644
--- a/ground/src/shared/uavobjectdefinition/ahrssettings.xml
+++ b/ground/src/shared/uavobjectdefinition/ahrssettings.xml
@@ -4,6 +4,7 @@
+