Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 00:35:34 +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;