AHRS: Paramaterize the variance used for the indoor algorithm.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1898 ebee16cc-31ac-478f-84a7-5cbb03baadba

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;

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)

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;

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

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;

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;

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

ground/src/shared/uavobjectdefinition/ahrssettings.xml

@@ -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"/>