From 0c30101f5476acf002bee100b441ef5d42948f5c Mon Sep 17 00:00:00 2001 From: peabody124 Date: Sat, 2 Oct 2010 03:10:01 +0000 Subject: [PATCH] AHRS: Made an object setting to allow the downsampling rate to be changed on the fly (EKF rate up to a limit). Also, now the algorithm selects if you are indoor or outdoor as well as if you use a mag indoor (if you do set the z variance higher than it calibrates to). git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1841 ebee16cc-31ac-478f-84a7-5cbb03baadba --- flight/AHRS/ahrs.c | 44 ++++++++++--------- flight/OpenPilot/UAVObjects/ahrssettings.c | 1 + flight/OpenPilot/UAVObjects/homelocation.c | 1 - .../OpenPilot/UAVObjects/inc/ahrssettings.h | 6 ++- .../OpenPilot/UAVObjects/inc/homelocation.h | 6 +-- .../OpenPilotOSX.xcodeproj/project.pbxproj | 2 + ground/src/plugins/config/ahrs.ui | 17 ------- .../src/plugins/config/configahrswidget.cpp | 12 ----- .../src/plugins/uavobjects/ahrssettings.cpp | 8 +++- ground/src/plugins/uavobjects/ahrssettings.h | 6 ++- ground/src/plugins/uavobjects/ahrssettings.py | 16 ++++++- .../src/plugins/uavobjects/homelocation.cpp | 7 --- ground/src/plugins/uavobjects/homelocation.h | 6 +-- ground/src/plugins/uavobjects/homelocation.py | 14 +----- .../uavobjectdefinition/ahrssettings.xml | 3 +- .../uavobjectdefinition/homelocation.xml | 1 - 16 files changed, 61 insertions(+), 89 deletions(-) diff --git a/flight/AHRS/ahrs.c b/flight/AHRS/ahrs.c index 468078fb8..2e2b59470 100644 --- a/flight/AHRS/ahrs.c +++ b/flight/AHRS/ahrs.c @@ -41,6 +41,7 @@ #include "CoordinateConversions.h" #include "ahrs_spi_comm.h" +#define MAX_OVERSAMPLING 50 // For debugging the raw sensors //#define DUMP_RAW //#define DUMP_FRIENDLY @@ -57,7 +58,7 @@ extern float Q[NUMW], R[NUMV]; // input noise and measurement noise variances extern float K[NUMX][NUMV]; // feedback gain matrix #endif -volatile enum algorithms ahrs_algorithm; +volatile int8_t ahrs_algorithm; /** * @addtogroup AHRS_Structures Local Structres @@ -175,7 +176,7 @@ uint32_t counter_val; * Public data. Used by both EKF and the sender */ //! Filter coefficients used in decimation. Limited order so filter can't run between samples -int16_t fir_coeffs[50]; +int16_t fir_coeffs[MAX_OVERSAMPLING]; //! The oversampling rate, ekf is 2k / this @@ -339,7 +340,7 @@ for all data to be up to date before doing anything*/ downsample_data(); /******************** INS ALGORITHM **************************/ - if (ahrs_algorithm == INSGPS_Algo) { + if (ahrs_algorithm != AHRSSETTINGS_ALGORITHM_SIMPLE) { // format data for INS algo gyro[0] = gyro_data.filtered.x; @@ -358,7 +359,7 @@ for all data to be up to date before doing anything*/ send_attitude(); // get message out quickly INSCovariancePrediction(1 / (float)EKF_RATE); - if (gps_data.updated && gps_data.quality == 1) { + if (gps_data.updated && ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) { // Compute velocity from Heading and groundspeed vel[0] = gps_data.groundspeed * @@ -384,7 +385,7 @@ for all data to be up to date before doing anything*/ gps_data.updated = false; mag_data.updated = 0; - } else if (gps_data.quality != -1 + } else if (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR && mag_data.updated == 1) { float mag_var[3] = {mag_data.calibration.variance[1], mag_data.calibration.variance[0], mag_data.calibration.variance[2]}; INSSetMagVar(mag_var); @@ -397,7 +398,7 @@ for all data to be up to date before doing anything*/ vel[1] = 0; vel[2] = 0; - if(mag_data.updated == 1) { + if((mag_data.updated == 1) && (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_INDOOR)) { float mag_var[3] = {10,10,10}; INSSetMagVar(mag_var); MagVelBaroCorrection(mag,vel,altitude_data.altitude); // only trust mags if outdoors @@ -795,14 +796,7 @@ void gps_callback(AhrsObjHandle obj) HomeLocationData home; HomeLocationGet(&home); - if(home.Set == HOMELOCATION_SET_FALSE || home.Indoor == HOMELOCATION_INDOOR_TRUE) { - gps_data.NED[0] = 0; - gps_data.NED[1] = 0; - gps_data.NED[2] = 0; - gps_data.groundspeed = 0; - gps_data.heading = 0; - gps_data.quality = -1; // indicates indoor mode, high variance zeros update - gps_data.updated = true; + if(ahrs_algorithm != AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) { return; } @@ -837,12 +831,22 @@ void settings_callback(AhrsObjHandle obj) AHRSSettingsData settings; AHRSSettingsGet(&settings); - if(settings.Algorithm == AHRSSETTINGS_ALGORITHM_INSGPS) - { - ahrs_algorithm = INSGPS_Algo; - }else - { - ahrs_algorithm = SIMPLE_Algo; + ahrs_algorithm = settings.Algorithm; + + if(settings.Downsampling != adc_oversampling) { + adc_oversampling = settings.Downsampling; + if(adc_oversampling > MAX_OVERSAMPLING) { + adc_oversampling = MAX_OVERSAMPLING; + settings.Downsampling = MAX_OVERSAMPLING; + AHRSSettingsSet(&settings); + } + AHRS_ADC_Config(adc_oversampling); + + /* Use simple averaging filter for now */ + for (int i = 0; i < adc_oversampling; i++) + fir_coeffs[i] = 1; + fir_coeffs[adc_oversampling] = adc_oversampling; + } } diff --git a/flight/OpenPilot/UAVObjects/ahrssettings.c b/flight/OpenPilot/UAVObjects/ahrssettings.c index 640e614ca..76d75575c 100644 --- a/flight/OpenPilot/UAVObjects/ahrssettings.c +++ b/flight/OpenPilot/UAVObjects/ahrssettings.c @@ -81,6 +81,7 @@ static void setDefaults(UAVObjHandle obj, uint16_t instId) UAVObjGetInstanceData(obj, instId, &data); memset(&data, 0, sizeof(AHRSSettingsData)); data.Algorithm = 1; + data.Downsampling = 20; data.UpdateRaw = 0; data.UpdateFiltered = 1; data.UpdatePeriod = 20; diff --git a/flight/OpenPilot/UAVObjects/homelocation.c b/flight/OpenPilot/UAVObjects/homelocation.c index 972e6a29a..17565788f 100644 --- a/flight/OpenPilot/UAVObjects/homelocation.c +++ b/flight/OpenPilot/UAVObjects/homelocation.c @@ -81,7 +81,6 @@ static void setDefaults(UAVObjHandle obj, uint16_t instId) UAVObjGetInstanceData(obj, instId, &data); memset(&data, 0, sizeof(HomeLocationData)); data.Set = 0; - data.Indoor = 0; data.Latitude = 0; data.Longitude = 0; data.Altitude = 0; diff --git a/flight/OpenPilot/UAVObjects/inc/ahrssettings.h b/flight/OpenPilot/UAVObjects/inc/ahrssettings.h index c6fb3efee..9eb111402 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 1565605328U +#define AHRSSETTINGS_OBJID 4141274898U #define AHRSSETTINGS_NAME "AHRSSettings" #define AHRSSETTINGS_METANAME "AHRSSettingsMeta" #define AHRSSETTINGS_ISSINGLEINST 1 @@ -72,6 +72,7 @@ // Object data typedef struct { uint8_t Algorithm; + uint8_t Downsampling; uint8_t UpdateRaw; uint8_t UpdateFiltered; int32_t UpdatePeriod; @@ -81,7 +82,8 @@ typedef struct { // Field information // Field Algorithm information /* Enumeration options for field Algorithm */ -typedef enum { AHRSSETTINGS_ALGORITHM_SIMPLE=0, AHRSSETTINGS_ALGORITHM_INSGPS=1 } AHRSSettingsAlgorithmOptions; +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 UpdateRaw information /* Enumeration options for field UpdateRaw */ typedef enum { AHRSSETTINGS_UPDATERAW_FALSE=0, AHRSSETTINGS_UPDATERAW_TRUE=1 } AHRSSettingsUpdateRawOptions; diff --git a/flight/OpenPilot/UAVObjects/inc/homelocation.h b/flight/OpenPilot/UAVObjects/inc/homelocation.h index 868bc96c2..8d9ccd04c 100644 --- a/flight/OpenPilot/UAVObjects/inc/homelocation.h +++ b/flight/OpenPilot/UAVObjects/inc/homelocation.h @@ -41,7 +41,7 @@ #define HOMELOCATION_H // Object constants -#define HOMELOCATION_OBJID 316455980U +#define HOMELOCATION_OBJID 3590360786U #define HOMELOCATION_NAME "HomeLocation" #define HOMELOCATION_METANAME "HomeLocationMeta" #define HOMELOCATION_ISSINGLEINST 1 @@ -72,7 +72,6 @@ // Object data typedef struct { uint8_t Set; - uint8_t Indoor; int32_t Latitude; int32_t Longitude; float Altitude; @@ -86,9 +85,6 @@ typedef struct { // Field Set information /* Enumeration options for field Set */ typedef enum { HOMELOCATION_SET_FALSE=0, HOMELOCATION_SET_TRUE=1 } HomeLocationSetOptions; -// Field Indoor information -/* Enumeration options for field Indoor */ -typedef enum { HOMELOCATION_INDOOR_FALSE=0, HOMELOCATION_INDOOR_TRUE=1 } HomeLocationIndoorOptions; // Field Latitude information // Field Longitude information // Field Altitude information diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index be8257b9e..d50735aa3 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -18,6 +18,7 @@ 6519133C1256C52B0039C0A3 /* ahrs_spi_program_master.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = ahrs_spi_program_master.h; sourceTree = ""; }; 6519133D1256C52B0039C0A3 /* ahrs_spi_program_slave.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = ahrs_spi_program_slave.h; sourceTree = ""; }; 6519133E1256C52B0039C0A3 /* ahrs_spi_program.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = ahrs_spi_program.h; sourceTree = ""; }; + 651913571256D0030039C0A3 /* ahrssettings.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = ahrssettings.h; sourceTree = ""; }; 651CF9E5120B5D8300EEFD70 /* pios_usb_hid_desc.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_usb_hid_desc.c; sourceTree = ""; }; 651CF9E6120B5D8300EEFD70 /* pios_usb_hid_istr.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_usb_hid_istr.c; sourceTree = ""; }; 651CF9E7120B5D8300EEFD70 /* pios_usb_hid_prop.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_usb_hid_prop.c; sourceTree = ""; }; @@ -7224,6 +7225,7 @@ 65E8EF8011EEA61E00BBF654 /* inc */ = { isa = PBXGroup; children = ( + 651913571256D0030039C0A3 /* ahrssettings.h */, 65E8EF8111EEA61E00BBF654 /* actuatorcommand.h */, 65E8EF8211EEA61E00BBF654 /* actuatordesired.h */, 65E8EF8311EEA61E00BBF654 /* actuatorsettings.h */, diff --git a/ground/src/plugins/config/ahrs.ui b/ground/src/plugins/config/ahrs.ui index d36191e3c..b8475c707 100644 --- a/ground/src/plugins/config/ahrs.ui +++ b/ground/src/plugins/config/ahrs.ui @@ -294,23 +294,6 @@ Disabled if there is no GPS fix. Save Position - - - - 380 - 400 - 201 - 22 - - - - Check this box if you are flying indoors and have no GPS fix. -Only makes sense with the "INSGPS" algorithm. - - - Indoor flight mode - - diff --git a/ground/src/plugins/config/configahrswidget.cpp b/ground/src/plugins/config/configahrswidget.cpp index 4ef01cbf5..55475a04e 100644 --- a/ground/src/plugins/config/configahrswidget.cpp +++ b/ground/src/plugins/config/configahrswidget.cpp @@ -689,9 +689,6 @@ void ConfigAHRSWidget::ahrsSettingsRequest() drawVariancesGraph(); obj = dynamic_cast(getObjectManager()->getObject(QString("HomeLocation"))); - field = obj->getField(QString("Indoor")); - if (field) - m_ahrs->indoorFlight->setChecked(field->getValue().toBool()); field = obj->getField(QString("Set")); if (field) m_ahrs->homeLocationSet->setEnabled(field->getValue().toBool()); @@ -712,10 +709,6 @@ void ConfigAHRSWidget::enableHomeLocSave(UAVObject * obj) if (field) { m_ahrs->homeLocationSet->setEnabled(field->getValue().toBool()); } - // While we're at it, ensure the 'indoor' flag is consistent: - field = obj->getField(QString("Indoor")); - if (field) - m_ahrs->indoorFlight->setChecked(field->getValue().toBool()); } @@ -729,11 +722,6 @@ void ConfigAHRSWidget::ahrsSettingsSaveRAM() field->setValue(m_ahrs->algorithm->currentText()); obj->updated(); obj = dynamic_cast(getObjectManager()->getObject(QString("HomeLocation"))); - field = obj->getField(QString("Indoor")); - if (m_ahrs->indoorFlight->isChecked()) - field->setValue(QString("TRUE")); - else - field->setValue(QString("FALSE")); field = obj->getField(QString("Set")); if (m_ahrs->homeLocationSet->isChecked()) field->setValue(QString("TRUE")); diff --git a/ground/src/plugins/uavobjects/ahrssettings.cpp b/ground/src/plugins/uavobjects/ahrssettings.cpp index 37ec86fde..94abd245b 100644 --- a/ground/src/plugins/uavobjects/ahrssettings.cpp +++ b/ground/src/plugins/uavobjects/ahrssettings.cpp @@ -46,8 +46,13 @@ AHRSSettings::AHRSSettings(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTINGS, NAM AlgorithmElemNames.append("0"); QStringList AlgorithmEnumOptions; AlgorithmEnumOptions.append("SIMPLE"); - AlgorithmEnumOptions.append("INSGPS"); + AlgorithmEnumOptions.append("INSGPS_INDOOR_NOMAG"); + AlgorithmEnumOptions.append("INSGPS_INDOOR"); + AlgorithmEnumOptions.append("INSGPS_OUTDOOR"); fields.append( new UAVObjectField(QString("Algorithm"), QString(""), UAVObjectField::ENUM, AlgorithmElemNames, AlgorithmEnumOptions) ); + QStringList DownsamplingElemNames; + DownsamplingElemNames.append("0"); + fields.append( new UAVObjectField(QString("Downsampling"), QString(""), UAVObjectField::UINT8, DownsamplingElemNames, QStringList()) ); QStringList UpdateRawElemNames; UpdateRawElemNames.append("0"); QStringList UpdateRawEnumOptions; @@ -97,6 +102,7 @@ UAVObject::Metadata AHRSSettings::getDefaultMetadata() void AHRSSettings::setDefaultFieldValues() { data.Algorithm = 1; + data.Downsampling = 20; data.UpdateRaw = 0; data.UpdateFiltered = 1; data.UpdatePeriod = 20; diff --git a/ground/src/plugins/uavobjects/ahrssettings.h b/ground/src/plugins/uavobjects/ahrssettings.h index 1aeaa3b18..ccb7123be 100644 --- a/ground/src/plugins/uavobjects/ahrssettings.h +++ b/ground/src/plugins/uavobjects/ahrssettings.h @@ -44,6 +44,7 @@ public: // Field structure typedef struct { quint8 Algorithm; + quint8 Downsampling; quint8 UpdateRaw; quint8 UpdateFiltered; qint32 UpdatePeriod; @@ -53,7 +54,8 @@ public: // Field information // Field Algorithm information /* Enumeration options for field Algorithm */ - typedef enum { ALGORITHM_SIMPLE=0, ALGORITHM_INSGPS=1 } AlgorithmOptions; + typedef enum { ALGORITHM_SIMPLE=0, ALGORITHM_INSGPS_INDOOR_NOMAG=1, ALGORITHM_INSGPS_INDOOR=2, ALGORITHM_INSGPS_OUTDOOR=3 } AlgorithmOptions; + // Field Downsampling information // Field UpdateRaw information /* Enumeration options for field UpdateRaw */ typedef enum { UPDATERAW_FALSE=0, UPDATERAW_TRUE=1 } UpdateRawOptions; @@ -64,7 +66,7 @@ public: // Constants - static const quint32 OBJID = 1565605328U; + static const quint32 OBJID = 4141274898U; 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 565e43390..3d6e4d560 100644 --- a/ground/src/plugins/uavobjects/ahrssettings.py +++ b/ground/src/plugins/uavobjects/ahrssettings.py @@ -46,7 +46,19 @@ _fields = [ \ ], { '0' : 'SIMPLE', - '1' : 'INSGPS', + '1' : 'INSGPS_INDOOR_NOMAG', + '2' : 'INSGPS_INDOOR', + '3' : 'INSGPS_OUTDOOR', + } + ), + uavobject.UAVObjectField( + 'Downsampling', + 'B', + 1, + [ + '0', + ], + { } ), uavobject.UAVObjectField( @@ -88,7 +100,7 @@ _fields = [ \ class AHRSSettings(uavobject.UAVObject): ## Object constants - OBJID = 1565605328 + OBJID = 4141274898 NAME = "AHRSSettings" METANAME = "AHRSSettingsMeta" ISSINGLEINST = 1 diff --git a/ground/src/plugins/uavobjects/homelocation.cpp b/ground/src/plugins/uavobjects/homelocation.cpp index f709c05c3..46c6a7b72 100644 --- a/ground/src/plugins/uavobjects/homelocation.cpp +++ b/ground/src/plugins/uavobjects/homelocation.cpp @@ -48,12 +48,6 @@ HomeLocation::HomeLocation(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTINGS, NAM SetEnumOptions.append("FALSE"); SetEnumOptions.append("TRUE"); fields.append( new UAVObjectField(QString("Set"), QString(""), UAVObjectField::ENUM, SetElemNames, SetEnumOptions) ); - QStringList IndoorElemNames; - IndoorElemNames.append("0"); - QStringList IndoorEnumOptions; - IndoorEnumOptions.append("FALSE"); - IndoorEnumOptions.append("TRUE"); - fields.append( new UAVObjectField(QString("Indoor"), QString(""), UAVObjectField::ENUM, IndoorElemNames, IndoorEnumOptions) ); QStringList LatitudeElemNames; LatitudeElemNames.append("0"); fields.append( new UAVObjectField(QString("Latitude"), QString("deg * 10e6"), UAVObjectField::INT32, LatitudeElemNames, QStringList()) ); @@ -118,7 +112,6 @@ UAVObject::Metadata HomeLocation::getDefaultMetadata() void HomeLocation::setDefaultFieldValues() { data.Set = 0; - data.Indoor = 0; data.Latitude = 0; data.Longitude = 0; data.Altitude = 0; diff --git a/ground/src/plugins/uavobjects/homelocation.h b/ground/src/plugins/uavobjects/homelocation.h index f596bc19d..1bcd8dcfc 100644 --- a/ground/src/plugins/uavobjects/homelocation.h +++ b/ground/src/plugins/uavobjects/homelocation.h @@ -44,7 +44,6 @@ public: // Field structure typedef struct { quint8 Set; - quint8 Indoor; qint32 Latitude; qint32 Longitude; float Altitude; @@ -58,9 +57,6 @@ public: // Field Set information /* Enumeration options for field Set */ typedef enum { SET_FALSE=0, SET_TRUE=1 } SetOptions; - // Field Indoor information - /* Enumeration options for field Indoor */ - typedef enum { INDOOR_FALSE=0, INDOOR_TRUE=1 } IndoorOptions; // Field Latitude information // Field Longitude information // Field Altitude information @@ -76,7 +72,7 @@ public: // Constants - static const quint32 OBJID = 316455980U; + static const quint32 OBJID = 3590360786U; static const QString NAME; static const bool ISSINGLEINST = 1; static const bool ISSETTINGS = 1; diff --git a/ground/src/plugins/uavobjects/homelocation.py b/ground/src/plugins/uavobjects/homelocation.py index 9d8c76ccc..6de803981 100644 --- a/ground/src/plugins/uavobjects/homelocation.py +++ b/ground/src/plugins/uavobjects/homelocation.py @@ -49,18 +49,6 @@ _fields = [ \ '1' : 'TRUE', } ), - uavobject.UAVObjectField( - 'Indoor', - 'b', - 1, - [ - '0', - ], - { - '0' : 'FALSE', - '1' : 'TRUE', - } - ), uavobject.UAVObjectField( 'Latitude', 'i', @@ -138,7 +126,7 @@ _fields = [ \ class HomeLocation(uavobject.UAVObject): ## Object constants - OBJID = 316455980 + OBJID = 3590360786 NAME = "HomeLocation" METANAME = "HomeLocationMeta" ISSINGLEINST = 1 diff --git a/ground/src/shared/uavobjectdefinition/ahrssettings.xml b/ground/src/shared/uavobjectdefinition/ahrssettings.xml index a94a527e1..5c0c7ea79 100644 --- a/ground/src/shared/uavobjectdefinition/ahrssettings.xml +++ b/ground/src/shared/uavobjectdefinition/ahrssettings.xml @@ -1,7 +1,8 @@ Settings for the @ref AHRSCommsModule to control the algorithm and what is updated - + + diff --git a/ground/src/shared/uavobjectdefinition/homelocation.xml b/ground/src/shared/uavobjectdefinition/homelocation.xml index a276f81c6..361f2550f 100644 --- a/ground/src/shared/uavobjectdefinition/homelocation.xml +++ b/ground/src/shared/uavobjectdefinition/homelocation.xml @@ -2,7 +2,6 @@ HomeLocation setting which contains the constants to tranlate from longitutde and latitude to NED reference frame. Automatically set by @ref GPSModule after acquiring a 3D lock. Used by @ref AHRSCommsModule. -