From d1e6dcc2f0b09f712ff59f4e56a9a7fa4f823e00 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Wed, 25 Jul 2012 10:50:32 -0500 Subject: [PATCH 01/12] First pass implementation of William Premerlani's magnetometer bias correction --- flight/Modules/Sensors/sensors.c | 60 ++++++++++++++++++- flight/Revolution/UAVObjects.inc | 1 + .../src/plugins/uavobjects/uavobjects.pro | 2 + shared/uavobjectdefinition/magbias.xml | 12 ++++ 4 files changed, 74 insertions(+), 1 deletion(-) create mode 100644 shared/uavobjectdefinition/magbias.xml diff --git a/flight/Modules/Sensors/sensors.c b/flight/Modules/Sensors/sensors.c index c5fe3f351..bb3b4bef9 100644 --- a/flight/Modules/Sensors/sensors.c +++ b/flight/Modules/Sensors/sensors.c @@ -49,6 +49,7 @@ #include "pios.h" #include "attitude.h" #include "magnetometer.h" +#include "magbias.h" #include "accels.h" #include "gyros.h" #include "gyrosbias.h" @@ -63,7 +64,7 @@ #include // Private constants -#define STACK_SIZE_BYTES 700 +#define STACK_SIZE_BYTES 1000 #define TASK_PRIORITY (tskIDLE_PRIORITY+3) #define SENSOR_PERIOD 2 @@ -80,6 +81,7 @@ static bool baro_updated = false; static void SensorsTask(void *parameters); static void settingsUpdatedCb(UAVObjEvent * objEv); static void sensorsUpdatedCb(UAVObjEvent * objEv); +static void magOffsetEstimation(MagnetometerData *mag); // These values are initialized by settings but can be updated by the attitude algorithm static bool bias_correct_gyro = true; @@ -111,6 +113,7 @@ int32_t SensorsInitialize(void) GyrosBiasInitialize(); AccelsInitialize(); MagnetometerInitialize(); + MagBiasInitialize(); RevoCalibrationInitialize(); AttitudeSettingsInitialize(); @@ -407,6 +410,9 @@ static void SensorsTask(void *parameters) mag.y = mags[1]; mag.z = mags[2]; } + + // Correct for mag bias and update + //magOffsetEstimation(&mag); MagnetometerSet(&mag); mag_update_time = PIOS_DELAY_GetRaw(); } @@ -418,6 +424,58 @@ static void SensorsTask(void *parameters) } } +/** + * Perform an update of the @ref MagBias based on + * Magnetometer Offset Cancellation: Theory and Implementation, + * revisited William Premerlani, October 14, 2011 + */ +static void magOffsetEstimation(MagnetometerData *mag) +{ + // Constants, to possibly go into a UAVO + static const int UPDATE_INTERVAL = 10; + static const float MIN_NORM_DIFFERENCE = 5; + static const float CONVERGENCE_RATE = 1.0f; + + static unsigned int call_count = 0; + static float B2[3] = {0, 0, 0}; + + call_count++; + + MagBiasData magBias; + MagBiasGet(&magBias); + + // Remove the current estimate of the bias + mag->x -= magBias.x; + mag->y -= magBias.y; + mag->z -= magBias.z; + + // First call + if (B2[0] == 0 && B2[1] == 0 && B2[2] == 0) { + B2[0] = mag->x; + B2[1] = mag->y; + B2[2] = mag->z; + return; + } + if (call_count % UPDATE_INTERVAL == 0) { + float B1[3] = {mag->x, mag->y, mag->z}; + float norm_diff = sqrtf(powf(B2[0] - B1[0],2) + powf(B2[1] - B1[1],2) + powf(B2[2] - B1[2],2)); + if (norm_diff > MIN_NORM_DIFFERENCE) { + float norm_b1 = sqrtf(B1[0]*B1[0] + B1[1]*B1[1] + B1[2]*B1[2]); + float norm_b2 = sqrtf(B2[0]*B2[0] + B2[1]*B2[1] + B2[2]*B2[2]); + float scale = CONVERGENCE_RATE * (norm_b2 - norm_b1) / norm_diff; + float b_error[3] = {(B2[0] - B1[0]) * scale, (B2[1] - B1[1]) * scale, (B2[2] - B1[2]) * scale}; + + magBias.x += b_error[0]; + magBias.y += b_error[1]; + magBias.z += b_error[2]; + MagBiasSet(&magBias); + } + + // Store this value to compare against next update + B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2]; + } +} + /** * Indicate that these sensors have been updated */ diff --git a/flight/Revolution/UAVObjects.inc b/flight/Revolution/UAVObjects.inc index be2b4b873..5b99daef4 100644 --- a/flight/Revolution/UAVObjects.inc +++ b/flight/Revolution/UAVObjects.inc @@ -35,6 +35,7 @@ UAVOBJSRCFILENAMES += gyros UAVOBJSRCFILENAMES += gyrosbias UAVOBJSRCFILENAMES += accels UAVOBJSRCFILENAMES += magnetometer +UAVOBJSRCFILENAMES += magbias UAVOBJSRCFILENAMES += baroaltitude UAVOBJSRCFILENAMES += baroairspeed UAVOBJSRCFILENAMES += fixedwingpathfollowersettings diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index d978d0814..70f120115 100755 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -38,6 +38,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/gyrosbias.h \ $$UAVOBJECT_SYNTHETICS/accels.h \ $$UAVOBJECT_SYNTHETICS/magnetometer.h \ + $$UAVOBJECT_SYNTHETICS/magbias.h \ $$UAVOBJECT_SYNTHETICS/camerastabsettings.h \ $$UAVOBJECT_SYNTHETICS/flighttelemetrystats.h \ $$UAVOBJECT_SYNTHETICS/fixedwingpathfollowersettings.h \ @@ -110,6 +111,7 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/gyros.cpp \ $$UAVOBJECT_SYNTHETICS/gyrosbias.cpp \ $$UAVOBJECT_SYNTHETICS/magnetometer.cpp \ + $$UAVOBJECT_SYNTHETICS/magbias.cpp \ $$UAVOBJECT_SYNTHETICS/camerastabsettings.cpp \ $$UAVOBJECT_SYNTHETICS/flighttelemetrystats.cpp \ $$UAVOBJECT_SYNTHETICS/systemstats.cpp \ diff --git a/shared/uavobjectdefinition/magbias.xml b/shared/uavobjectdefinition/magbias.xml new file mode 100644 index 000000000..63319466f --- /dev/null +++ b/shared/uavobjectdefinition/magbias.xml @@ -0,0 +1,12 @@ + + + The gyro data. + + + + + + + + + From 1a5af9dafbcb56f5796e44edd988087252a30159 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Wed, 25 Jul 2012 11:09:38 -0500 Subject: [PATCH 02/12] Remove some old debugging code --- flight/Modules/Sensors/sensors.c | 22 ---------------------- 1 file changed, 22 deletions(-) diff --git a/flight/Modules/Sensors/sensors.c b/flight/Modules/Sensors/sensors.c index bb3b4bef9..8e865c5a7 100644 --- a/flight/Modules/Sensors/sensors.c +++ b/flight/Modules/Sensors/sensors.c @@ -57,8 +57,6 @@ #include "attitudesettings.h" #include "revocalibration.h" #include "flightstatus.h" -#include "gpsposition.h" -#include "baroaltitude.h" #include "CoordinateConversions.h" #include @@ -74,13 +72,10 @@ // Private variables static xTaskHandle sensorsTaskHandle; -static bool gps_updated = false; -static bool baro_updated = false; // Private functions static void SensorsTask(void *parameters); static void settingsUpdatedCb(UAVObjEvent * objEv); -static void sensorsUpdatedCb(UAVObjEvent * objEv); static void magOffsetEstimation(MagnetometerData *mag); // These values are initialized by settings but can be updated by the attitude algorithm @@ -209,12 +204,6 @@ static void SensorsTask(void *parameters) } } - // If debugging connect callback - if(pios_com_aux_id != 0) { - BaroAltitudeConnectCallback(&sensorsUpdatedCb); - GPSPositionConnectCallback(&sensorsUpdatedCb); - } - // Main task loop lastSysTime = xTaskGetTickCount(); bool error = false; @@ -476,17 +465,6 @@ static void magOffsetEstimation(MagnetometerData *mag) } } -/** - * Indicate that these sensors have been updated - */ -static void sensorsUpdatedCb(UAVObjEvent * objEv) -{ - if(objEv->obj == GPSPositionHandle()) - gps_updated = true; - if(objEv->obj == BaroAltitudeHandle()) - baro_updated = true; -} - /** * Locally cache some variables from the AtttitudeSettings object */ From 3b57b492d059e4a2e189d58875c913edfbc6a943 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Wed, 25 Jul 2012 11:19:39 -0500 Subject: [PATCH 03/12] Mag bias tracking seems to work --- flight/Modules/Sensors/sensors.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/flight/Modules/Sensors/sensors.c b/flight/Modules/Sensors/sensors.c index 8e865c5a7..36546de30 100644 --- a/flight/Modules/Sensors/sensors.c +++ b/flight/Modules/Sensors/sensors.c @@ -401,7 +401,7 @@ static void SensorsTask(void *parameters) } // Correct for mag bias and update - //magOffsetEstimation(&mag); + magOffsetEstimation(&mag); MagnetometerSet(&mag); mag_update_time = PIOS_DELAY_GetRaw(); } @@ -457,6 +457,7 @@ static void magOffsetEstimation(MagnetometerData *mag) magBias.x += b_error[0]; magBias.y += b_error[1]; magBias.z += b_error[2]; + MagBiasSet(&magBias); } From c587ceebfdb75b44b95fe1aa3b60d19dbf1e58fa Mon Sep 17 00:00:00 2001 From: James Cotton Date: Wed, 25 Jul 2012 11:23:27 -0500 Subject: [PATCH 04/12] Make the mag offset nulling convergence rate come from the UAVO. When it is set to zero nulling does not occur to allow us to still calibrate the sensors. --- flight/Modules/Sensors/sensors.c | 25 +++++++++++++------ .../uavobjectdefinition/revocalibration.xml | 6 ++++- 2 files changed, 23 insertions(+), 8 deletions(-) diff --git a/flight/Modules/Sensors/sensors.c b/flight/Modules/Sensors/sensors.c index 36546de30..f062fd148 100644 --- a/flight/Modules/Sensors/sensors.c +++ b/flight/Modules/Sensors/sensors.c @@ -70,14 +70,16 @@ #define PI_MOD(x) (fmodf(x + F_PI, F_PI * 2) - F_PI) // Private types -// Private variables -static xTaskHandle sensorsTaskHandle; // Private functions static void SensorsTask(void *parameters); static void settingsUpdatedCb(UAVObjEvent * objEv); static void magOffsetEstimation(MagnetometerData *mag); +// Private variables +static xTaskHandle sensorsTaskHandle; +RevoCalibrationData cal; + // These values are initialized by settings but can be updated by the attitude algorithm static bool bias_correct_gyro = true; @@ -400,8 +402,10 @@ static void SensorsTask(void *parameters) mag.z = mags[2]; } - // Correct for mag bias and update - magOffsetEstimation(&mag); + // Correct for mag bias and update if the rate is non zero + if(cal.MagBiasNullingRate > 0) + magOffsetEstimation(&mag); + MagnetometerSet(&mag); mag_update_time = PIOS_DELAY_GetRaw(); } @@ -423,7 +427,6 @@ static void magOffsetEstimation(MagnetometerData *mag) // Constants, to possibly go into a UAVO static const int UPDATE_INTERVAL = 10; static const float MIN_NORM_DIFFERENCE = 5; - static const float CONVERGENCE_RATE = 1.0f; static unsigned int call_count = 0; static float B2[3] = {0, 0, 0}; @@ -451,7 +454,7 @@ static void magOffsetEstimation(MagnetometerData *mag) if (norm_diff > MIN_NORM_DIFFERENCE) { float norm_b1 = sqrtf(B1[0]*B1[0] + B1[1]*B1[1] + B1[2]*B1[2]); float norm_b2 = sqrtf(B2[0]*B2[0] + B2[1]*B2[1] + B2[2]*B2[2]); - float scale = CONVERGENCE_RATE * (norm_b2 - norm_b1) / norm_diff; + float scale = cal.MagBiasNullingRate * (norm_b2 - norm_b1) / norm_diff; float b_error[3] = {(B2[0] - B1[0]) * scale, (B2[1] - B1[1]) * scale, (B2[2] - B1[2]) * scale}; magBias.x += b_error[0]; @@ -470,7 +473,6 @@ static void magOffsetEstimation(MagnetometerData *mag) * Locally cache some variables from the AtttitudeSettings object */ static void settingsUpdatedCb(UAVObjEvent * objEv) { - RevoCalibrationData cal; RevoCalibrationGet(&cal); mag_bias[0] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_X]; @@ -487,6 +489,15 @@ static void settingsUpdatedCb(UAVObjEvent * objEv) { accel_scale[2] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Z]; // Do not store gyros_bias here as that comes from the state estimator and should be // used from GyroBias directly + + // Zero out any adaptive tracking + MagBiasData magBias; + MagBiasGet(&magBias); + magBias.x = 0; + magBias.y = 0; + magBias.z = 0; + MagBiasSet(&magBias); + AttitudeSettingsData attitudeSettings; AttitudeSettingsGet(&attitudeSettings); diff --git a/shared/uavobjectdefinition/revocalibration.xml b/shared/uavobjectdefinition/revocalibration.xml index ee5722b6f..481190b5a 100644 --- a/shared/uavobjectdefinition/revocalibration.xml +++ b/shared/uavobjectdefinition/revocalibration.xml @@ -1,7 +1,7 @@ Settings for the INS to control the algorithm and what is updated - + @@ -18,6 +18,10 @@ + + + + From 41f80eb34f60d2636a28edb0ef634a85f1105b99 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Wed, 25 Jul 2012 11:26:37 -0500 Subject: [PATCH 05/12] Disable offset nulling while performing GCS mag calibration. --- ground/openpilotgcs/src/plugins/config/configrevowidget.cpp | 6 ++++++ ground/openpilotgcs/src/plugins/config/configrevowidget.h | 1 + 2 files changed, 7 insertions(+) diff --git a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp index 84be939d0..0f28dba51 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp @@ -535,6 +535,10 @@ void ConfigRevoWidget::doStartSixPointCalibration() revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y] = 0; revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z] = 0; + // Disable adaptive mag nulling + initialMagCorrectionRate = revoCalibrationData.MagBiasNullingRate; + revoCalibrationData.MagBiasNullingRate = 0; + revoCalibration->setData(revoCalibrationData); Thread::usleep(100000); @@ -740,6 +744,8 @@ void ConfigRevoWidget::computeScaleBias() revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y] = -sign(S[1]) * b[1]; revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z] = -sign(S[2]) * b[2]; + // Restore the previous setting + revoCalibrationData.MagBiasNullingRate = initialMagCorrectionRate; #ifdef SIX_POINT_CAL_ACCEL bool good_calibration = true; diff --git a/ground/openpilotgcs/src/plugins/config/configrevowidget.h b/ground/openpilotgcs/src/plugins/config/configrevowidget.h index ae5bcf830..7621c6fb7 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevowidget.h +++ b/ground/openpilotgcs/src/plugins/config/configrevowidget.h @@ -95,6 +95,7 @@ private: UAVObject::Metadata initialGyrosMdata; UAVObject::Metadata initialMagMdata; UAVObject::Metadata initialBaroMdata; + float initialMagCorrectionRate; int position; From 735c0098436f56c79d486eab327da327ccf1a3bf Mon Sep 17 00:00:00 2001 From: James Cotton Date: Wed, 25 Jul 2012 13:14:59 -0500 Subject: [PATCH 06/12] Disable writing to the overo driver for every pios_com write again. It drives up system consumption too much, although now data will get lost :(. --- flight/PiOS/STM32F4xx/pios_overo.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/flight/PiOS/STM32F4xx/pios_overo.c b/flight/PiOS/STM32F4xx/pios_overo.c index d45a461fc..c535d413e 100644 --- a/flight/PiOS/STM32F4xx/pios_overo.c +++ b/flight/PiOS/STM32F4xx/pios_overo.c @@ -127,7 +127,7 @@ static void PIOS_OVERO_WriteData(struct pios_overo_dev *overo_dev) bytes_added = (overo_dev->tx_out_cb)(overo_dev->tx_out_context, writing_pointer, max_bytes, NULL, &tx_need_yield); -#if OVERO_USES_BLOCKING_WRITE +#if defined(OVERO_USES_BLOCKING_WRITE) if (tx_need_yield) { vPortYieldFromISR(); } @@ -326,7 +326,7 @@ static void PIOS_OVERO_TxStart(uint32_t overo_id, uint16_t tx_bytes_avail) // DMA TX enable (enable IRQ) ? // Load any pending bytes from TX fifo - PIOS_OVERO_WriteData(overo_dev); + //PIOS_OVERO_WriteData(overo_dev); } static void PIOS_OVERO_RegisterRxCallback(uint32_t overo_id, pios_com_callback rx_in_cb, uint32_t context) From 0ed5b84ee9d1f76e38552f084c27534bd757a522 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Wed, 25 Jul 2012 14:41:10 -0500 Subject: [PATCH 07/12] Move the try catch when parsing overo files --- .../src/plugins/uavobjects/uavobjecttemplate.m | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjecttemplate.m b/ground/openpilotgcs/src/plugins/uavobjects/uavobjecttemplate.m index 0f9ead8a5..3dd62df2f 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjecttemplate.m +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjecttemplate.m @@ -75,14 +75,15 @@ startTime=clock; while (1) if (feof(fid)); break; end + try %% Read message header % get sync field (0x3C, 1 byte) sync = fread(fid, 1, 'uint8'); if sync ~= correctSyncByte - prebuf = [prebuf(2:end); sync]; - wrongSyncByte = wrongSyncByte + 1; - continue - end + prebuf = [prebuf(2:end); sync]; + wrongSyncByte = wrongSyncByte + 1; + continue + end % get msg type (quint8 1 byte ) should be 0x20, ignore the rest? msgType = fread(fid, 1, 'uint8'); @@ -109,7 +110,7 @@ while (1) end %% Read object - try + switch objID $(SWITCHCODE) otherwise From 880d58e4d913ab1bd74803abdad7bfa8f1105a1e Mon Sep 17 00:00:00 2001 From: James Cotton Date: Thu, 26 Jul 2012 16:43:24 -0500 Subject: [PATCH 08/12] Changes to mag nulling from D-Lite. Perform update only when we have a new vector sufficiently different from the previous one. --- flight/Modules/Sensors/sensors.c | 33 ++++++++++++++------------------ 1 file changed, 14 insertions(+), 19 deletions(-) diff --git a/flight/Modules/Sensors/sensors.c b/flight/Modules/Sensors/sensors.c index f062fd148..deb7145ba 100644 --- a/flight/Modules/Sensors/sensors.c +++ b/flight/Modules/Sensors/sensors.c @@ -425,14 +425,10 @@ static void SensorsTask(void *parameters) static void magOffsetEstimation(MagnetometerData *mag) { // Constants, to possibly go into a UAVO - static const int UPDATE_INTERVAL = 10; - static const float MIN_NORM_DIFFERENCE = 5; + static const float MIN_NORM_DIFFERENCE = 50; - static unsigned int call_count = 0; static float B2[3] = {0, 0, 0}; - call_count++; - MagBiasData magBias; MagBiasGet(&magBias); @@ -448,21 +444,20 @@ static void magOffsetEstimation(MagnetometerData *mag) B2[2] = mag->z; return; } - if (call_count % UPDATE_INTERVAL == 0) { - float B1[3] = {mag->x, mag->y, mag->z}; - float norm_diff = sqrtf(powf(B2[0] - B1[0],2) + powf(B2[1] - B1[1],2) + powf(B2[2] - B1[2],2)); - if (norm_diff > MIN_NORM_DIFFERENCE) { - float norm_b1 = sqrtf(B1[0]*B1[0] + B1[1]*B1[1] + B1[2]*B1[2]); - float norm_b2 = sqrtf(B2[0]*B2[0] + B2[1]*B2[1] + B2[2]*B2[2]); - float scale = cal.MagBiasNullingRate * (norm_b2 - norm_b1) / norm_diff; - float b_error[3] = {(B2[0] - B1[0]) * scale, (B2[1] - B1[1]) * scale, (B2[2] - B1[2]) * scale}; - magBias.x += b_error[0]; - magBias.y += b_error[1]; - magBias.z += b_error[2]; - - MagBiasSet(&magBias); - } + float B1[3] = {mag->x, mag->y, mag->z}; + float norm_diff = sqrtf(powf(B2[0] - B1[0],2) + powf(B2[1] - B1[1],2) + powf(B2[2] - B1[2],2)); + if (norm_diff > MIN_NORM_DIFFERENCE) { + float norm_b1 = sqrtf(B1[0]*B1[0] + B1[1]*B1[1] + B1[2]*B1[2]); + float norm_b2 = sqrtf(B2[0]*B2[0] + B2[1]*B2[1] + B2[2]*B2[2]); + float scale = cal.MagBiasNullingRate * (norm_b2 - norm_b1) / norm_diff; + float b_error[3] = {(B2[0] - B1[0]) * scale, (B2[1] - B1[1]) * scale, (B2[2] - B1[2]) * scale}; + + magBias.x += b_error[0]; + magBias.y += b_error[1]; + magBias.z += b_error[2]; + + MagBiasSet(&magBias); // Store this value to compare against next update B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2]; From 9f00eda1b46852040fd4a6f8c3d59ba81b290058 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Thu, 26 Jul 2012 21:00:30 -0500 Subject: [PATCH 09/12] Add the mag offset compensation into into the simulated sensor code --- flight/Modules/Sensors/simulated/sensors.c | 65 +++++++++++++++++++++- 1 file changed, 62 insertions(+), 3 deletions(-) diff --git a/flight/Modules/Sensors/simulated/sensors.c b/flight/Modules/Sensors/simulated/sensors.c index 933e95279..62695e908 100644 --- a/flight/Modules/Sensors/simulated/sensors.c +++ b/flight/Modules/Sensors/simulated/sensors.c @@ -63,6 +63,7 @@ #include "gpsvelocity.h" #include "homelocation.h" #include "magnetometer.h" +#include "magbias.h" #include "ratedesired.h" #include "revocalibration.h" @@ -87,6 +88,8 @@ static void simulateModelAgnostic(); static void simulateModelQuadcopter(); static void simulateModelAirplane(); +static void magOffsetEstimation(MagnetometerData *mag); + static float accel_bias[3]; static float rand_gauss(); @@ -114,6 +117,7 @@ int32_t SensorsInitialize(void) GPSPositionInitialize(); GPSVelocityInitialize(); MagnetometerInitialize(); + MagBiasInitialize(); RevoCalibrationInitialize(); return 0; @@ -503,6 +507,10 @@ static void simulateModelQuadcopter() mag.x = homeLocation.Be[0] * Rbe[0][0] + homeLocation.Be[1] * Rbe[0][1] + homeLocation.Be[2] * Rbe[0][2]; mag.y = homeLocation.Be[0] * Rbe[1][0] + homeLocation.Be[1] * Rbe[1][1] + homeLocation.Be[2] * Rbe[1][2]; mag.z = homeLocation.Be[0] * Rbe[2][0] + homeLocation.Be[1] * Rbe[2][1] + homeLocation.Be[2] * Rbe[2][2]; + + // Run the offset compensation algorithm from the firmware + magOffsetEstimation(&mag); + MagnetometerSet(&mag); last_mag_time = PIOS_DELAY_GetRaw(); } @@ -779,9 +787,10 @@ static void simulateModelAirplane() static uint32_t last_mag_time = 0; if(PIOS_DELAY_DiffuS(last_mag_time) / 1.0e6 > MAG_PERIOD) { MagnetometerData mag; - mag.x = homeLocation.Be[0] * Rbe[0][0] + homeLocation.Be[1] * Rbe[0][1] + homeLocation.Be[2] * Rbe[0][2]; - mag.y = homeLocation.Be[0] * Rbe[1][0] + homeLocation.Be[1] * Rbe[1][1] + homeLocation.Be[2] * Rbe[1][2]; - mag.z = homeLocation.Be[0] * Rbe[2][0] + homeLocation.Be[1] * Rbe[2][1] + homeLocation.Be[2] * Rbe[2][2]; + mag.x = 100+homeLocation.Be[0] * Rbe[0][0] + homeLocation.Be[1] * Rbe[0][1] + homeLocation.Be[2] * Rbe[0][2]; + mag.y = 100+homeLocation.Be[0] * Rbe[1][0] + homeLocation.Be[1] * Rbe[1][1] + homeLocation.Be[2] * Rbe[1][2]; + mag.z = 100+homeLocation.Be[0] * Rbe[2][0] + homeLocation.Be[1] * Rbe[2][1] + homeLocation.Be[2] * Rbe[2][2]; + magOffsetEstimation(&mag); MagnetometerSet(&mag); last_mag_time = PIOS_DELAY_GetRaw(); } @@ -818,6 +827,56 @@ static float rand_gauss (void) { return (v1*sqrt(-2.0 * log(s) / s)); } +/** + * Perform an update of the @ref MagBias based on + * Magnetometer Offset Cancellation: Theory and Implementation, + * revisited William Premerlani, October 14, 2011 + */ +static void magOffsetEstimation(MagnetometerData *mag) +{ + RevoCalibrationData cal; + RevoCalibrationGet(&cal); + + // Constants, to possibly go into a UAVO + static const float MIN_NORM_DIFFERENCE = 50; + + static float B2[3] = {0, 0, 0}; + + MagBiasData magBias; + MagBiasGet(&magBias); + + // Remove the current estimate of the bias + mag->x -= magBias.x; + mag->y -= magBias.y; + mag->z -= magBias.z; + + // First call + if (B2[0] == 0 && B2[1] == 0 && B2[2] == 0) { + B2[0] = mag->x; + B2[1] = mag->y; + B2[2] = mag->z; + return; + } + + float B1[3] = {mag->x, mag->y, mag->z}; + float norm_diff = sqrtf(powf(B2[0] - B1[0],2) + powf(B2[1] - B1[1],2) + powf(B2[2] - B1[2],2)); + if (norm_diff > MIN_NORM_DIFFERENCE) { + float norm_b1 = sqrtf(B1[0]*B1[0] + B1[1]*B1[1] + B1[2]*B1[2]); + float norm_b2 = sqrtf(B2[0]*B2[0] + B2[1]*B2[1] + B2[2]*B2[2]); + float scale = cal.MagBiasNullingRate * (norm_b2 - norm_b1) / norm_diff; + float b_error[3] = {(B2[0] - B1[0]) * scale, (B2[1] - B1[1]) * scale, (B2[2] - B1[2]) * scale}; + + magBias.x += b_error[0]; + magBias.y += b_error[1]; + magBias.z += b_error[2]; + + MagBiasSet(&magBias); + + // Store this value to compare against next update + B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2]; + } +} + /** * @} * @} From abb0caa6bd1e437d9d953595b6b76a521ce7d01f Mon Sep 17 00:00:00 2001 From: James Cotton Date: Fri, 27 Jul 2012 13:14:18 -0500 Subject: [PATCH 10/12] Try a different mag nulling algorithm. --- flight/Modules/Sensors/sensors.c | 57 ++++++++++++++++++++++ flight/Modules/Sensors/simulated/sensors.c | 51 +++++++++++++++++++ 2 files changed, 108 insertions(+) diff --git a/flight/Modules/Sensors/sensors.c b/flight/Modules/Sensors/sensors.c index deb7145ba..7fe95f636 100644 --- a/flight/Modules/Sensors/sensors.c +++ b/flight/Modules/Sensors/sensors.c @@ -48,6 +48,7 @@ #include "pios.h" #include "attitude.h" +#include "homelocation.h" #include "magnetometer.h" #include "magbias.h" #include "accels.h" @@ -424,6 +425,7 @@ static void SensorsTask(void *parameters) */ static void magOffsetEstimation(MagnetometerData *mag) { +#if 0 // Constants, to possibly go into a UAVO static const float MIN_NORM_DIFFERENCE = 50; @@ -462,6 +464,61 @@ static void magOffsetEstimation(MagnetometerData *mag) // Store this value to compare against next update B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2]; } +#else + static uint32_t call_count = 0; + + MagBiasData magBias; + MagBiasGet(&magBias); + + // Remove the current estimate of the bias + mag->x -= magBias.x; + mag->y -= magBias.y; + mag->z -= magBias.z; + + if((call_count++ % 5) == 0) { + HomeLocationData homeLocation; + HomeLocationGet(&homeLocation); + + AttitudeActualData attitude; + AttitudeActualGet(&attitude); + + const float Rxy = sqrtf(homeLocation.Be[0]*homeLocation.Be[0] + homeLocation.Be[1]*homeLocation.Be[1]); + const float Rz = homeLocation.Be[2]; + + const float rate = cal.MagBiasNullingRate; + float R[3][3]; + float B_e[3]; + float xy[2]; + float delta[3]; + + // Get the rotation matrix + Quaternion2R(&attitude.q1, R); + + // Rotate the mag into the NED frame + B_e[0] = R[0][0] * mag->x + R[1][0] * mag->y + R[2][0] * mag->z; + B_e[1] = R[0][1] * mag->x + R[1][1] * mag->y + R[2][1] * mag->z; + B_e[2] = R[0][2] * mag->x + R[1][2] * mag->y + R[2][2] * mag->z; + + float cy = cosf(attitude.Yaw * M_PI / 180.0f); + float sy = sinf(attitude.Yaw * M_PI / 180.0f); + + xy[0] = cy * B_e[0] + sy * B_e[1]; + xy[1] = -sy * B_e[0] + cy * B_e[1]; + + float xy_norm = sqrtf(xy[0]*xy[0] + xy[1]*xy[1]); + + delta[0] = -rate * (xy[0] / xy_norm * Rxy - xy[0]); + delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]); + delta[2] = -rate * (Rz - B_e[2]); + + if (delta[0] == delta[0] && delta[1] == delta[1] && delta[2] == delta[2]) { + magBias.x += delta[0]; + magBias.y += delta[1]; + magBias.z += delta[2]; + MagBiasSet(&magBias); + } + } +#endif } /** diff --git a/flight/Modules/Sensors/simulated/sensors.c b/flight/Modules/Sensors/simulated/sensors.c index 62695e908..9dadd7477 100644 --- a/flight/Modules/Sensors/simulated/sensors.c +++ b/flight/Modules/Sensors/simulated/sensors.c @@ -834,6 +834,7 @@ static float rand_gauss (void) { */ static void magOffsetEstimation(MagnetometerData *mag) { +#if 0 RevoCalibrationData cal; RevoCalibrationGet(&cal); @@ -875,6 +876,56 @@ static void magOffsetEstimation(MagnetometerData *mag) // Store this value to compare against next update B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2]; } +#else + HomeLocationData homeLocation; + HomeLocationGet(&homeLocation); + + AttitudeActualData attitude; + AttitudeActualGet(&attitude); + + MagBiasData magBias; + MagBiasGet(&magBias); + + // Remove the current estimate of the bias + mag->x -= magBias.x; + mag->y -= magBias.y; + mag->z -= magBias.z; + + const float Rxy = sqrtf(homeLocation.Be[0]*homeLocation.Be[0] + homeLocation.Be[1]*homeLocation.Be[1]); + const float Rz = homeLocation.Be[2]; + + const float rate = 0.01; + float R[3][3]; + float B_e[3]; + float xy[2]; + float delta[3]; + + // Get the rotation matrix + Quaternion2R(&attitude.q1, R); + + // Rotate the mag into the NED frame + B_e[0] = R[0][0] * mag->x + R[1][0] * mag->y + R[2][0] * mag->z; + B_e[1] = R[0][1] * mag->x + R[1][1] * mag->y + R[2][1] * mag->z; + B_e[2] = R[0][2] * mag->x + R[1][2] * mag->y + R[2][2] * mag->z; + + float cy = cosf(attitude.Yaw * M_PI / 180.0f); + float sy = sinf(attitude.Yaw * M_PI / 180.0f); + + xy[0] = cy * B_e[0] + sy * B_e[1]; + xy[1] = -sy * B_e[0] + cy * B_e[1]; + + float xy_norm = sqrtf(xy[0]*xy[0] + xy[1]*xy[1]); + + delta[0] = -rate * (xy[0] / xy_norm * Rxy - xy[0]); + delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]); + delta[2] = -rate * (Rz - B_e[2]); + + magBias.x += delta[0]; + magBias.y += delta[1]; + magBias.z += delta[2]; + MagBiasSet(&magBias); +#endif + } /** From 221ae05ef6c405c6ad6e33dc810876eb39e77fe5 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Fri, 27 Jul 2012 13:20:30 -0500 Subject: [PATCH 11/12] Perform the update every cycle. Mag rate of 0.001 seems to work well. --- flight/Modules/Sensors/sensors.c | 86 +++++++++++++++----------------- 1 file changed, 41 insertions(+), 45 deletions(-) diff --git a/flight/Modules/Sensors/sensors.c b/flight/Modules/Sensors/sensors.c index 7fe95f636..22d22b250 100644 --- a/flight/Modules/Sensors/sensors.c +++ b/flight/Modules/Sensors/sensors.c @@ -465,8 +465,6 @@ static void magOffsetEstimation(MagnetometerData *mag) B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2]; } #else - static uint32_t call_count = 0; - MagBiasData magBias; MagBiasGet(&magBias); @@ -474,49 +472,47 @@ static void magOffsetEstimation(MagnetometerData *mag) mag->x -= magBias.x; mag->y -= magBias.y; mag->z -= magBias.z; - - if((call_count++ % 5) == 0) { - HomeLocationData homeLocation; - HomeLocationGet(&homeLocation); - - AttitudeActualData attitude; - AttitudeActualGet(&attitude); - - const float Rxy = sqrtf(homeLocation.Be[0]*homeLocation.Be[0] + homeLocation.Be[1]*homeLocation.Be[1]); - const float Rz = homeLocation.Be[2]; - - const float rate = cal.MagBiasNullingRate; - float R[3][3]; - float B_e[3]; - float xy[2]; - float delta[3]; - - // Get the rotation matrix - Quaternion2R(&attitude.q1, R); - - // Rotate the mag into the NED frame - B_e[0] = R[0][0] * mag->x + R[1][0] * mag->y + R[2][0] * mag->z; - B_e[1] = R[0][1] * mag->x + R[1][1] * mag->y + R[2][1] * mag->z; - B_e[2] = R[0][2] * mag->x + R[1][2] * mag->y + R[2][2] * mag->z; - - float cy = cosf(attitude.Yaw * M_PI / 180.0f); - float sy = sinf(attitude.Yaw * M_PI / 180.0f); - - xy[0] = cy * B_e[0] + sy * B_e[1]; - xy[1] = -sy * B_e[0] + cy * B_e[1]; - - float xy_norm = sqrtf(xy[0]*xy[0] + xy[1]*xy[1]); - - delta[0] = -rate * (xy[0] / xy_norm * Rxy - xy[0]); - delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]); - delta[2] = -rate * (Rz - B_e[2]); - - if (delta[0] == delta[0] && delta[1] == delta[1] && delta[2] == delta[2]) { - magBias.x += delta[0]; - magBias.y += delta[1]; - magBias.z += delta[2]; - MagBiasSet(&magBias); - } + + HomeLocationData homeLocation; + HomeLocationGet(&homeLocation); + + AttitudeActualData attitude; + AttitudeActualGet(&attitude); + + const float Rxy = sqrtf(homeLocation.Be[0]*homeLocation.Be[0] + homeLocation.Be[1]*homeLocation.Be[1]); + const float Rz = homeLocation.Be[2]; + + const float rate = cal.MagBiasNullingRate; + float R[3][3]; + float B_e[3]; + float xy[2]; + float delta[3]; + + // Get the rotation matrix + Quaternion2R(&attitude.q1, R); + + // Rotate the mag into the NED frame + B_e[0] = R[0][0] * mag->x + R[1][0] * mag->y + R[2][0] * mag->z; + B_e[1] = R[0][1] * mag->x + R[1][1] * mag->y + R[2][1] * mag->z; + B_e[2] = R[0][2] * mag->x + R[1][2] * mag->y + R[2][2] * mag->z; + + float cy = cosf(attitude.Yaw * M_PI / 180.0f); + float sy = sinf(attitude.Yaw * M_PI / 180.0f); + + xy[0] = cy * B_e[0] + sy * B_e[1]; + xy[1] = -sy * B_e[0] + cy * B_e[1]; + + float xy_norm = sqrtf(xy[0]*xy[0] + xy[1]*xy[1]); + + delta[0] = -rate * (xy[0] / xy_norm * Rxy - xy[0]); + delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]); + delta[2] = -rate * (Rz - B_e[2]); + + if (delta[0] == delta[0] && delta[1] == delta[1] && delta[2] == delta[2]) { + magBias.x += delta[0]; + magBias.y += delta[1]; + magBias.z += delta[2]; + MagBiasSet(&magBias); } #endif } From f02aacfeb6147757df6e5754bed0947f916aa0f0 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 28 Jul 2012 11:22:24 -0500 Subject: [PATCH 12/12] Change the constant so LED flashing works properly on Revo --- flight/PiOS/Common/pios_flash_jedec.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/flight/PiOS/Common/pios_flash_jedec.c b/flight/PiOS/Common/pios_flash_jedec.c index ed4a6390b..4af779f1a 100644 --- a/flight/PiOS/Common/pios_flash_jedec.c +++ b/flight/PiOS/Common/pios_flash_jedec.c @@ -333,8 +333,11 @@ int32_t PIOS_Flash_Jedec_EraseChip() while(PIOS_Flash_Jedec_Busy() != 0) { #if defined(FLASH_FREERTOS) vTaskDelay(1); -#endif + if ((i++) % 100 == 0) +#else if ((i++) % 10000 == 0) +#endif + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); }