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

Improvements

Browse source
This commit is contained in:
Scavanger 2023-11-24 08:34:43 -03:00
parent 02806b17f7
commit 3a3f507a94
10 changed files with 200 additions and 32 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -2668,6 +2668,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
case MSP_SET_WP:
if (dataSize == 21) {
static uint8_t mmIdx = 0;
const uint8_t msp_wp_no = sbufReadU8(src); // get the waypoint number
navWaypoint_t msp_wp;
msp_wp.action = sbufReadU8(src); // action
@ -2679,6 +2680,17 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
msp_wp.p3 = sbufReadU16(src); // P3
msp_wp.flag = sbufReadU8(src); // future: to set nav flag
setWaypoint(msp_wp_no, &msp_wp);
uint8_t fwAppraochStartIdx = 8;
#ifdef USE_SAFE_HOME
fwAppraochStartIdx = MAX_SAFE_HOMES;
#endif
if (msp_wp_no == 0) {
mmIdx = 0;
} else if (msp_wp.flag == NAV_WP_FLAG_LAST) {
mmIdx++;
}
resetFwAutolandApproach(fwAppraochStartIdx + mmIdx);
} else
return MSP_RESULT_ERROR;
break;
@ -3137,6 +3149,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
safeHomeConfigMutable(i)->enabled = sbufReadU8(src);
safeHomeConfigMutable(i)->lat = sbufReadU32(src);
safeHomeConfigMutable(i)->lon = sbufReadU32(src);
resetFwAutolandApproach(i);
} else {
return MSP_RESULT_ERROR;
}