AHRS: Add a configuration to export the sensor data either with or without INS

bias correction.  This is useful for stabilization to get rid of the gyro bias.
Making it an option is important for calibration.

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

flight/AHRS/ahrs.c

@@ -638,9 +638,9 @@ void downsample_data()
 	raw.gyrotemp[0] = valid_data_buffer[6];
 	raw.gyrotemp[1] = valid_data_buffer[7];
 
-	raw.gyros_filtered[0] = (gyro_data.filtered.x - Nav.gyro_bias[0]) * 180 / M_PI;
+	raw.gyros_filtered[0] = gyro_data.filtered.x * 180 / M_PI;
-	raw.gyros_filtered[1] = (gyro_data.filtered.y - Nav.gyro_bias[1]) * 180 / M_PI;
+	raw.gyros_filtered[1] = gyro_data.filtered.y * 180 / M_PI;
-	raw.gyros_filtered[2] = (gyro_data.filtered.z - Nav.gyro_bias[2]) * 180 / M_PI;
+	raw.gyros_filtered[2] = gyro_data.filtered.z * 180 / M_PI;
 
 	raw.accels[0] = valid_data_buffer[2];
 	raw.accels[1] = valid_data_buffer[0];
@@ -654,6 +654,17 @@ void downsample_data()
 	raw.magnetometers[1] = mag_data.scaled.axis[1];
 	raw.magnetometers[2] = mag_data.scaled.axis[2];
 
+	AHRSSettingsData settings;
+	AHRSSettingsGet(&settings);
+	if(settings.BiasCorrectedRaw == AHRSSETTINGS_BIASCORRECTEDRAW_TRUE) {
+		raw.gyros_filtered[0] -= Nav.gyro_bias[0] * 180 / M_PI;
+		raw.gyros_filtered[1] -= Nav.gyro_bias[1] * 180 / M_PI;
+		raw.gyros_filtered[2] -= Nav.gyro_bias[2] * 180 / M_PI;
+		raw.accels_filtered[0] -= Nav.accel_bias[0];
+		raw.accels_filtered[1] -= Nav.accel_bias[1];
+		raw.accels_filtered[2] -= Nav.accel_bias[2];
+	}
+
 	AttitudeRawSet(&raw);
 }
 

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.BiasCorrectedRaw = 0;
 	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 3741078856U
 #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;
+    uint8_t BiasCorrectedRaw;
     float YawBias;
     float PitchBias;
     float RollBias;
@@ -86,6 +87,9 @@ 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 BiasCorrectedRaw information
+/* Enumeration options for field BiasCorrectedRaw */
+typedef enum { AHRSSETTINGS_BIASCORRECTEDRAW_TRUE=0, AHRSSETTINGS_BIASCORRECTEDRAW_FALSE=1 } AHRSSettingsBiasCorrectedRawOptions;
 // Field YawBias information
 // Field PitchBias information
 // Field RollBias information

ground/src/plugins/config/configahrswidget.cpp

@@ -557,6 +557,12 @@ void ConfigAHRSWidget::computeScaleBias()
 
     obj->updated();
 
+    obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSSettings")));
+    field = obj->getField(QString("BiasCorrectedRaw"));
+    field->setValue("TRUE");
+    obj->updated();
+
+
     position = -1; //set to run again
     m_ahrs->sixPointCalibInstructions->append("Computed accel and mag scale and bias...");
 
@@ -597,6 +603,12 @@ void ConfigAHRSWidget::sixPointCalibrationMode()
 
     obj->updated();
 
+    obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSSettings")));
+    field = obj->getField(QString("BiasCorrectedRaw"));
+    field->setValue("FALSE");
+    obj->updated();
+
+
     Thread::usleep(100000);
 
     gyro_accum_x.clear();

ground/src/plugins/uavobjects/ahrssettings.cpp

@@ -56,6 +56,12 @@ 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 BiasCorrectedRawElemNames;
+    BiasCorrectedRawElemNames.append("0");
+    QStringList BiasCorrectedRawEnumOptions;
+    BiasCorrectedRawEnumOptions.append("TRUE");
+    BiasCorrectedRawEnumOptions.append("FALSE");
+    fields.append( new UAVObjectField(QString("BiasCorrectedRaw"), QString(""), UAVObjectField::ENUM, BiasCorrectedRawElemNames, BiasCorrectedRawEnumOptions) );
     QStringList YawBiasElemNames;
     YawBiasElemNames.append("0");
     fields.append( new UAVObjectField(QString("YawBias"), QString(""), UAVObjectField::FLOAT32, YawBiasElemNames, QStringList()) );
@@ -101,6 +107,7 @@ void AHRSSettings::setDefaultFieldValues()
     data.Algorithm = 1;
     data.Downsampling = 20;
     data.UpdatePeriod = 1;
+    data.BiasCorrectedRaw = 0;
     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;
+        quint8 BiasCorrectedRaw;
         float YawBias;
         float PitchBias;
         float RollBias;
@@ -58,13 +59,16 @@ 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 BiasCorrectedRaw information
+    /* Enumeration options for field BiasCorrectedRaw */
+    typedef enum { BIASCORRECTEDRAW_TRUE=0, BIASCORRECTEDRAW_FALSE=1 } BiasCorrectedRawOptions;
     // Field YawBias information
     // Field PitchBias information
     // Field RollBias information
 
   
     // Constants
-    static const quint32 OBJID = 1456050280U;
+    static const quint32 OBJID = 3741078856U;
     static const QString NAME;
     static const bool ISSINGLEINST = 1;
     static const bool ISSETTINGS = 1;

ground/src/plugins/uavobjects/ahrssettings.py

@@ -71,6 +71,18 @@ _fields = [ \
 		{
 		}
 	),
+	uavobject.UAVObjectField(
+		'BiasCorrectedRaw',
+		'b',
+		1,
+		[
+			'0',
+		],
+		{
+			'0' : 'TRUE',
+			'1' : 'FALSE',
+		}
+	),
 	uavobject.UAVObjectField(
 		'YawBias',
 		'f',
@@ -106,7 +118,7 @@ _fields = [ \
 
 class AHRSSettings(uavobject.UAVObject):
     ## Object constants
-    OBJID        = 1456050280
+    OBJID        = 3741078856
     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="BiasCorrectedRaw" units="" type="enum" elements="1" options="TRUE,FALSE" defaultvalue="TRUE"/>
         <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"/>