Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 17:25:18 +03:00
Code Issues Wiki Activity

Add magGain to MSP frame

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2020-07-24 18:28:44 +02:00
parent 8ff3072cf1
commit 3bdc3dd501
2 changed files with 28 additions and 1 deletions
Download patch file Download diff file Expand all files Collapse all files

24
src/main/fc/fc_msp.c
View file

@ -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;

5
src/main/sensors/compass.c
View file

@ -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 {