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

Move appraoch to extra config

This commit is contained in:
Scavanger 2023-11-17 10:43:40 -03:00
parent cdc1a05e7c
commit 02806b17f7
11 changed files with 193 additions and 616 deletions

View file

@ -1653,12 +1653,6 @@ static mspResult_e mspFcSafeHomeOutCommand(sbuf_t *dst, sbuf_t *src)
sbufWriteU8(dst, safeHomeConfig(safe_home_no)->enabled);
sbufWriteU32(dst, safeHomeConfig(safe_home_no)->lat);
sbufWriteU32(dst, safeHomeConfig(safe_home_no)->lon);
sbufWriteU32(dst, safeHomeConfig(safe_home_no)->approachAlt);
sbufWriteU32(dst, safeHomeConfig(safe_home_no)->landAlt);
sbufWriteU8(dst, safeHomeConfig(safe_home_no)->approachDirection);
sbufWriteU16(dst, safeHomeConfig(safe_home_no)->landApproachHeading1);
sbufWriteU16(dst, safeHomeConfig(safe_home_no)->landApproachHeading2);
sbufWriteU8(dst, safeHomeConfig(safe_home_no)->isSeaLevelRef);
return MSP_RESULT_ACK;
} else {
return MSP_RESULT_ERROR;
@ -1666,6 +1660,22 @@ static mspResult_e mspFcSafeHomeOutCommand(sbuf_t *dst, sbuf_t *src)
}
#endif
static mspResult_e mspFwApproachOutCommand(sbuf_t *dst, sbuf_t *src)
{
const uint8_t idx = sbufReadU8(src);
if(idx < MAX_FW_LAND_APPOACH_SETTINGS) {
sbufWriteU8(dst, idx);
sbufWriteU32(dst, fwAutolandApproachConfig(idx)->approachAlt);
sbufWriteU32(dst, fwAutolandApproachConfig(idx)->landAlt);
sbufWriteU8(dst, fwAutolandApproachConfig(idx)->approachDirection);
sbufWriteU16(dst, fwAutolandApproachConfig(idx)->landApproachHeading1);
sbufWriteU16(dst, fwAutolandApproachConfig(idx)->landApproachHeading2);
sbufWriteU8(dst, fwAutolandApproachConfig(idx)->isSeaLevelRef);
return MSP_RESULT_ACK;
} else {
return MSP_RESULT_ERROR;
}
}
static mspResult_e mspFcLogicConditionCommand(sbuf_t *dst, sbuf_t *src) {
const uint8_t idx = sbufReadU8(src);
@ -3119,7 +3129,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
#endif
#ifdef USE_SAFE_HOME
case MSP2_INAV_SET_SAFEHOME:
if (dataSize == 24) {
if (dataSize == 10) {
uint8_t i;
if (!sbufReadU8Safe(&i, src) || i >= MAX_SAFE_HOMES) {
return MSP_RESULT_ERROR;
@ -3127,18 +3137,30 @@ 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);
safeHomeConfigMutable(i)->approachAlt = sbufReadU32(src);
safeHomeConfigMutable(i)->landAlt = sbufReadU32(src);
safeHomeConfigMutable(i)->approachDirection = sbufReadU8(src);
} else {
return MSP_RESULT_ERROR;
}
break;
#endif
case MSP2_INAV_SET_FW_APPROACH:
if (dataSize == 15) {
uint8_t i;
if (!sbufReadU8Safe(&i, src) || i >= MAX_FW_LAND_APPOACH_SETTINGS) {
return MSP_RESULT_ERROR;
}
fwAutolandApproachConfigMutable(i)->approachAlt = sbufReadU32(src);
fwAutolandApproachConfigMutable(i)->landAlt = sbufReadU32(src);
fwAutolandApproachConfigMutable(i)->approachDirection = sbufReadU8(src);
int16_t head1 = 0, head2 = 0;
if (sbufReadI16Safe(&head1, src)) {
safeHomeConfigMutable(i)->landApproachHeading1 = head1;
fwAutolandApproachConfigMutable(i)->landApproachHeading1 = head1;
}
if (sbufReadI16Safe(&head2, src)) {
safeHomeConfigMutable(i)->landApproachHeading2 = head2;
fwAutolandApproachConfigMutable(i)->landApproachHeading2 = head2;
}
safeHomeConfigMutable(i)->isSeaLevelRef = sbufReadU8(src);
fwAutolandApproachConfigMutable(i)->isSeaLevelRef = sbufReadU8(src);
} else {
return MSP_RESULT_ERROR;
}
@ -3657,6 +3679,9 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
*ret = mspFcSafeHomeOutCommand(dst, src);
break;
#endif
case MSP2_INAV_FW_APPROACH:
*ret = mspFwApproachOutCommand(dst, src);
break;
#ifdef USE_SIMULATOR
case MSP_SIMULATOR: