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 #else
sbufWriteU16(dst, 0); sbufWriteU16(dst, 0);
#endif #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; break;
case MSP_POSITION_ESTIMATION_CONFIG: case MSP_POSITION_ESTIMATION_CONFIG:
@ -2190,6 +2201,19 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
if (dataSize >= 20) { if (dataSize >= 20) {
opticalFlowConfigMutable()->opflow_scale = sbufReadU16(src) / 256.0f; 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 #endif
} else } else
return MSP_RESULT_ERROR; 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}; static int magGain[XYZ_AXIS_COUNT] = {-4096, -4096, -4096};
// Check magZero // 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); DISABLE_STATE(COMPASS_CALIBRATED);
} }
else { else {