From 6f1e7b4e41f02189f7dc34e514eb3486b0a044e4 Mon Sep 17 00:00:00 2001 From: peabody124 Date: Sat, 4 Dec 2010 17:34:25 +0000 Subject: [PATCH] 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 | 17 ++++++++++++++--- flight/OpenPilot/UAVObjects/ahrssettings.c | 1 + flight/OpenPilot/UAVObjects/inc/ahrssettings.h | 6 +++++- ground/src/plugins/config/configahrswidget.cpp | 12 ++++++++++++ ground/src/plugins/uavobjects/ahrssettings.cpp | 7 +++++++ ground/src/plugins/uavobjects/ahrssettings.h | 6 +++++- ground/src/plugins/uavobjects/ahrssettings.py | 14 +++++++++++++- .../shared/uavobjectdefinition/ahrssettings.xml | 1 + 8 files changed, 58 insertions(+), 6 deletions(-) diff --git a/flight/AHRS/ahrs.c b/flight/AHRS/ahrs.c index 524c73168..b7ad3f798 100644 --- a/flight/AHRS/ahrs.c +++ b/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[1] = (gyro_data.filtered.y - Nav.gyro_bias[1]) * 180 / M_PI; - raw.gyros_filtered[2] = (gyro_data.filtered.z - Nav.gyro_bias[2]) * 180 / M_PI; + raw.gyros_filtered[0] = gyro_data.filtered.x * 180 / M_PI; + raw.gyros_filtered[1] = gyro_data.filtered.y * 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); } diff --git a/flight/OpenPilot/UAVObjects/ahrssettings.c b/flight/OpenPilot/UAVObjects/ahrssettings.c index 5c5c33783..972b980ab 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.BiasCorrectedRaw = 0; 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..7baf59ce7 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 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 diff --git a/ground/src/plugins/config/configahrswidget.cpp b/ground/src/plugins/config/configahrswidget.cpp index bff295dea..5d88d7379 100644 --- a/ground/src/plugins/config/configahrswidget.cpp +++ b/ground/src/plugins/config/configahrswidget.cpp @@ -557,6 +557,12 @@ void ConfigAHRSWidget::computeScaleBias() obj->updated(); + obj = dynamic_cast(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(getObjectManager()->getObject(QString("AHRSSettings"))); + field = obj->getField(QString("BiasCorrectedRaw")); + field->setValue("FALSE"); + obj->updated(); + + Thread::usleep(100000); gyro_accum_x.clear(); diff --git a/ground/src/plugins/uavobjects/ahrssettings.cpp b/ground/src/plugins/uavobjects/ahrssettings.cpp index eae5c6e8f..c0d8aa311 100644 --- a/ground/src/plugins/uavobjects/ahrssettings.cpp +++ b/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; diff --git a/ground/src/plugins/uavobjects/ahrssettings.h b/ground/src/plugins/uavobjects/ahrssettings.h index 2ba86f6d9..a910cdde4 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; + 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; diff --git a/ground/src/plugins/uavobjects/ahrssettings.py b/ground/src/plugins/uavobjects/ahrssettings.py index b2b020bf9..a1ca3367b 100644 --- a/ground/src/plugins/uavobjects/ahrssettings.py +++ b/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 diff --git a/ground/src/shared/uavobjectdefinition/ahrssettings.xml b/ground/src/shared/uavobjectdefinition/ahrssettings.xml index 9d278bdb6..d5756cde8 100644 --- a/ground/src/shared/uavobjectdefinition/ahrssettings.xml +++ b/ground/src/shared/uavobjectdefinition/ahrssettings.xml @@ -4,6 +4,7 @@ +