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

Merge pull request #1158 from iNavFlight/msp-position-estimation

Browse source 
MSP_POSITION_ESTIMATION_CONFIG
This commit is contained in:
Martin Budden 2017-01-24 10:09:18 +00:00 • committed by GitHub
commit fa50058fac
2 changed files with 37 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1201,6 +1201,28 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
#endif
break;
case MSP_POSITION_ESTIMATION_CONFIG:
#ifdef NAV
sbufWriteU16(dst, positionEstimationConfig()->w_z_baro_p * 100); // inav_w_z_baro_p float as value * 100
sbufWriteU16(dst, positionEstimationConfig()->w_z_gps_p * 100); // 2 inav_w_z_gps_p float as value * 100
sbufWriteU16(dst, positionEstimationConfig()->w_z_gps_v * 100); // 2 inav_w_z_gps_v float as value * 100
sbufWriteU16(dst, positionEstimationConfig()->w_xy_gps_p * 100); // 2 inav_w_xy_gps_p float as value * 100
sbufWriteU16(dst, positionEstimationConfig()->w_xy_gps_v * 100); // 2 inav_w_xy_gps_v float as value * 100
sbufWriteU8(dst, positionEstimationConfig()->gps_min_sats); // 1 inav_gps_min_sats
sbufWriteU8(dst, positionEstimationConfig()->use_gps_velned); // 1 inav_use_gps_velned ON/OFF
#else
sbufWriteU16(dst, 0);
sbufWriteU16(dst, 0);
sbufWriteU16(dst, 0);
sbufWriteU16(dst, 0);
sbufWriteU16(dst, 0);
sbufWriteU8(dst, 0);
sbufWriteU8(dst, 0);
#endif
break;
case MSP_REBOOT:
if (!ARMING_FLAG(ARMED)) {
if (mspPostProcessFn) {
@ -1652,6 +1674,18 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
#endif
break;
case MSP_SET_POSITION_ESTIMATION_CONFIG:
#ifdef NAV
positionEstimationConfigMutable()->w_z_baro_p = constrainf(sbufReadU16(src) / 100.0f, 0.0f, 10.0f);
positionEstimationConfigMutable()->w_z_gps_p = constrainf(sbufReadU16(src) / 100.0f, 0.0f, 10.0f);
positionEstimationConfigMutable()->w_z_gps_v = constrainf(sbufReadU16(src) / 100.0f, 0.0f, 10.0f);
positionEstimationConfigMutable()->w_xy_gps_p = constrainf(sbufReadU16(src) / 100.0f, 0.0f, 10.0f);
positionEstimationConfigMutable()->w_xy_gps_v = constrainf(sbufReadU16(src) / 100.0f, 0.0f, 10.0f);
positionEstimationConfigMutable()->gps_min_sats = constrain(sbufReadU8(src), 5, 10);
positionEstimationConfigMutable()->use_gps_velned = constrain(sbufReadU8(src), 0, 1);
#endif
break;
case MSP_RESET_CONF:
if (!ARMING_FLAG(ARMED)) {
resetEEPROM();