From e7410ea2206bca806e95a4f45cf28a1240af80d7 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 3 Mar 2019 19:52:36 +0100 Subject: [PATCH 1/6] Experimental calibration for MAG gain --- src/main/fc/settings.yaml | 12 ++++++++++++ src/main/sensors/compass.c | 26 +++++++++++++++++++++----- src/main/sensors/compass.h | 1 + 3 files changed, 34 insertions(+), 5 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 12d6f9a367c..5e8272a0bb2 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -303,6 +303,18 @@ groups: field: magZero.raw[Z] min: INT16_MIN max: INT16_MAX + - name: maggain_x + field: magGain[X] + min: INT16_MIN + max: INT16_MAX + - name: maggain_y + field: magGain[Y] + min: INT16_MIN + max: INT16_MAX + - name: maggain_z + field: magGain[Z] + min: INT16_MIN + max: INT16_MAX - name: mag_calibration_time field: magCalibrationTimeLimit min: 30 diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 0a0173dce82..6d8ae40b62f 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -57,7 +57,7 @@ mag_t mag; // mag access functions -PG_REGISTER_WITH_RESET_TEMPLATE(compassConfig_t, compassConfig, PG_COMPASS_CONFIG, 3); +PG_REGISTER_WITH_RESET_TEMPLATE(compassConfig_t, compassConfig, PG_COMPASS_CONFIG, 4); #ifdef USE_MAG #define MAG_HARDWARE_DEFAULT MAG_AUTODETECT @@ -334,6 +334,7 @@ void compassUpdate(timeUs_t currentTimeUs) static sensorCalibrationState_t calState; static timeUs_t calStartedAt = 0; static int16_t magPrev[XYZ_AXIS_COUNT]; + static int magGain[XYZ_AXIS_COUNT] = {-4096, -4096, -4096}; // Check magZero if ((compassConfig()->magZero.raw[X] == 0) && (compassConfig()->magZero.raw[Y] == 0) && (compassConfig()->magZero.raw[Z] == 0)) { @@ -375,9 +376,15 @@ void compassUpdate(timeUs_t currentTimeUs) float diffMag = 0; float avgMag = 0; - for (int axis = 0; axis < 3; axis++) { + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { diffMag += (mag.magADC[axis] - magPrev[axis]) * (mag.magADC[axis] - magPrev[axis]); avgMag += (mag.magADC[axis] + magPrev[axis]) * (mag.magADC[axis] + magPrev[axis]) / 4.0f; + + int32_t sample = ABS(mag.magADC[axis]); + if (sample > magGain[axis]) { + magGain[axis] = sample; + } + } // sqrtf(diffMag / avgMag) is a rough approximation of tangent of angle between magADC and magPrev. tan(8 deg) = 0.14 @@ -396,14 +403,23 @@ void compassUpdate(timeUs_t currentTimeUs) compassConfigMutable()->magZero.raw[axis] = lrintf(magZerof[axis]); } + /* + * Scale calibration + * We use max absolute value of each axis as scale calibration with constant 1024 as base + * It is dirty, but worth checking if this will solve the problem of changing mag vector when UAV is tilted + */ + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + compassConfigMutable()->magGain[axis] = magGain[axis] - compassConfig()->magZero.raw[axis]; + } + calStartedAt = 0; saveConfigAndNotify(); } } else { - mag.magADC[X] -= compassConfig()->magZero.raw[X]; - mag.magADC[Y] -= compassConfig()->magZero.raw[Y]; - mag.magADC[Z] -= compassConfig()->magZero.raw[Z]; + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + mag.magADC[axis] = (mag.magADC[axis] - compassConfig()->magZero.raw[axis]) * compassConfig()->magGain[axis] / 1024; + } } if (mag.dev.magAlign.useExternal) { diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index 87cbfaa48cf..7c9a0fca63b 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -58,6 +58,7 @@ typedef struct compassConfig_s { sensor_align_e mag_align; // on-board mag alignment. Ignored if externally aligned via *DeciDegrees. uint8_t mag_hardware; // Which mag hardware to use on boards with more than one device flightDynamicsTrims_t magZero; + int16_t magGain[XYZ_AXIS_COUNT]; uint8_t mag_to_use; uint8_t magCalibrationTimeLimit; // Time for compass calibration (seconds) int16_t rollDeciDegrees; // Alignment for external mag on the roll (X) axis (0.1deg) From b66a7d7be31a7b0e6e77204ec3862d3e4464b9f5 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 4 Mar 2019 09:07:34 +0100 Subject: [PATCH 2/6] Fix math --- src/main/sensors/compass.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 6d8ae40b62f..eda1fab967a 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -418,7 +418,7 @@ void compassUpdate(timeUs_t currentTimeUs) } else { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - mag.magADC[axis] = (mag.magADC[axis] - compassConfig()->magZero.raw[axis]) * compassConfig()->magGain[axis] / 1024; + mag.magADC[axis] = (mag.magADC[axis] - compassConfig()->magZero.raw[axis]) * 1024 / compassConfig()->magGain[axis]; } } From b09eba7aabb4341b156661c277b36b57943d5ead Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 13 Jul 2020 13:22:18 +0200 Subject: [PATCH 3/6] Docs improvements --- src/main/fc/settings.yaml | 6 ++++++ src/main/sensors/compass.c | 2 +- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 2046e7d0149..780070e75fc 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -419,14 +419,20 @@ groups: min: INT16_MIN max: INT16_MAX - name: maggain_x + description: "Magnetometer calibration X gain. If 0, no calibration or calibration failed" + default_value: "0" field: magGain[X] min: INT16_MIN max: INT16_MAX - name: maggain_y + description: "Magnetometer calibration Y gain. If 0, no calibration or calibration failed" + default_value: "0" field: magGain[Y] min: INT16_MIN max: INT16_MAX - name: maggain_z + description: "Magnetometer calibration Z gain. If 0, no calibration or calibration failed" + default_value: "0" field: magGain[Z] min: INT16_MIN max: INT16_MAX diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index eda1fab967a..90fb2c5ac0c 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -380,7 +380,7 @@ void compassUpdate(timeUs_t currentTimeUs) diffMag += (mag.magADC[axis] - magPrev[axis]) * (mag.magADC[axis] - magPrev[axis]); avgMag += (mag.magADC[axis] + magPrev[axis]) * (mag.magADC[axis] + magPrev[axis]) / 4.0f; - int32_t sample = ABS(mag.magADC[axis]); + const int32_t sample = ABS(mag.magADC[axis]); if (sample > magGain[axis]) { magGain[axis] = sample; } From 3bdc3dd501313e603abb92ab384baf1dc9ea27e1 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 24 Jul 2020 18:28:44 +0200 Subject: [PATCH 4/6] Add magGain to MSP frame --- src/main/fc/fc_msp.c | 24 ++++++++++++++++++++++++ src/main/sensors/compass.c | 5 ++++- 2 files changed, 28 insertions(+), 1 deletion(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 076b7077f1d..716e3ad49b3 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1283,6 +1283,17 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF #else sbufWriteU16(dst, 0); #endif + + #ifdef USE_MAG + sbufWriteU16(dst, compassConfig()->magGain[X]); + sbufWriteU16(dst, compassConfig()->magGain[Y]); + sbufWriteU16(dst, compassConfig()->magGain[Z]); + #else + sbufWriteU16(dst, 0); + sbufWriteU16(dst, 0); + sbufWriteU16(dst, 0); + #endif + break; case MSP_POSITION_ESTIMATION_CONFIG: @@ -2190,6 +2201,19 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) if (dataSize >= 20) { opticalFlowConfigMutable()->opflow_scale = sbufReadU16(src) / 256.0f; } +#endif +#ifdef USE_MAG + if (dataSize >= 22) { + compassConfigMutable()->magGain[X] = sbufReadU16(src); + compassConfigMutable()->magGain[Y] = sbufReadU16(src); + compassConfigMutable()->magGain[Z] = sbufReadU16(src); + } +#else + if (dataSize >= 22) { + sbufReadU16(src); + sbufReadU16(src); + sbufReadU16(src); + } #endif } else return MSP_RESULT_ERROR; diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 90fb2c5ac0c..87841fecd8f 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -337,7 +337,10 @@ void compassUpdate(timeUs_t currentTimeUs) static int magGain[XYZ_AXIS_COUNT] = {-4096, -4096, -4096}; // Check magZero - if ((compassConfig()->magZero.raw[X] == 0) && (compassConfig()->magZero.raw[Y] == 0) && (compassConfig()->magZero.raw[Z] == 0)) { + if ( + (compassConfig()->magZero.raw[X] == 0 && compassConfig()->magZero.raw[Y] == 0 && compassConfig()->magZero.raw[Z] == 0) || + compassConfig()->magGain[X] == 0 || compassConfig()->magGain[Y] == 0 || compassConfig()->magGain[Z] == 0 + ) { DISABLE_STATE(COMPASS_CALIBRATED); } else { From cf3db04fda6fd289f65764056d0f0d9d890dd35b Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 24 Jul 2020 18:57:21 +0200 Subject: [PATCH 5/6] Zero gain on mag calibration start --- src/main/sensors/compass.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 87841fecd8f..37314601bd5 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -363,6 +363,7 @@ void compassUpdate(timeUs_t currentTimeUs) for (int axis = 0; axis < 3; axis++) { compassConfigMutable()->magZero.raw[axis] = 0; + compassConfigMutable()->magGain[axis] = 0; magPrev[axis] = 0; } From 0df8dee3c9ca5c92025a428bc9a6795bcd04f808 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 14 Aug 2020 16:54:23 +0200 Subject: [PATCH 6/6] Disable KFC32F3 --- make/release.mk | 2 +- make/targets.mk | 2 +- src/main/target/KFC32F3_INAV/{target.mk => target.mk_} | 0 3 files changed, 2 insertions(+), 2 deletions(-) rename src/main/target/KFC32F3_INAV/{target.mk => target.mk_} (100%) diff --git a/make/release.mk b/make/release.mk index b00ac8b4bc1..ecc5955e0cb 100644 --- a/make/release.mk +++ b/make/release.mk @@ -1,6 +1,6 @@ RELEASE_TARGETS = AIRHEROF3 AIRHEROF3_QUAD -RELEASE_TARGETS += LUX_RACE RCEXPLORERF3 RMDO SPARKY KFC32F3_INAV FALCORE MOTOLAB ANYFC BLUEJAYF4 COLIBRI F4BY DALRCF405 DALRCF722DUAL +RELEASE_TARGETS += LUX_RACE RCEXPLORERF3 RMDO SPARKY FALCORE MOTOLAB ANYFC BLUEJAYF4 COLIBRI F4BY DALRCF405 DALRCF722DUAL RELEASE_TARGETS += QUANTON REVO SPARKY2 PIKOBLX CLRACINGF4AIR CLRACINGF4AIRV2 PIXRACER BEEROTORF4 ANYFCF7 ANYFCF7_EXTERNAL_BARO RELEASE_TARGETS += ALIENFLIGHTNGF7 diff --git a/make/targets.mk b/make/targets.mk index c725539ea6c..f3150c9c7b2 100644 --- a/make/targets.mk +++ b/make/targets.mk @@ -77,7 +77,7 @@ GROUP_3_TARGETS := AIRBOTF4 BLUEJAYF4 OMNIBUSF4 OMNIBUSF4PRO FIREWORKSV2 SPARKY2 GROUP_4_TARGETS := ANYFC ANYFCF7 ANYFCF7_EXTERNAL_BARO ALIENFLIGHTNGF7 PIXRACER YUPIF7 MATEKF405SE MATEKF411 MATEKF722 MATEKF405OSD MATEKF405_SERVOS6 NOX GROUP_5_TARGETS := ASGARD32F7 CLRACINGF4AIRV3 DALRCF405 DALRCF722DUAL DYSF4PROV2 F4BY FISHDRONEF4 FOXEERF405 FOXEERF722DUAL FRSKYF3 FRSKYF4 FURYF4OSD GROUP_6_TARGETS := MAMBAF405 OMNIBUSF4V3 OMNIBUSF4V3_S6_SS OMNIBUSF4V3_S5S6_SS OMNIBUSF4V3_S5_S6_2SS AIKONF4 -GROUP_7_TARGETS := KAKUTEF4 KAKUTEF4V2 KAKUTEF7 KAKUTEF7MINI KFC32F3_INAV MATEKF411_RSSI MATEKF411_SFTSRL2 MATEKF722MINI MATEKF722SE MATEKF722_HEXSERVO +GROUP_7_TARGETS := KAKUTEF4 KAKUTEF4V2 KAKUTEF7 KAKUTEF7MINI MATEKF411_RSSI MATEKF411_SFTSRL2 MATEKF722MINI MATEKF722SE MATEKF722_HEXSERVO GROUP_8_TARGETS := MATEKF765 MATEKF722PX KAKUTEF7HDV ZEEZF7 FOXEERF722V2 FRSKYPILOT FRSKY_ROVERF7 GROUP_OTHER_TARGETS := $(filter-out $(GROUP_1_TARGETS) $(GROUP_2_TARGETS) $(GROUP_3_TARGETS) $(GROUP_4_TARGETS) $(GROUP_5_TARGETS) $(GROUP_6_TARGETS) $(GROUP_7_TARGETS) $(GROUP_8_TARGETS), $(VALID_TARGETS)) diff --git a/src/main/target/KFC32F3_INAV/target.mk b/src/main/target/KFC32F3_INAV/target.mk_ similarity index 100% rename from src/main/target/KFC32F3_INAV/target.mk rename to src/main/target/KFC32F3_INAV/target.mk_