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:
parent
cdc1a05e7c
commit
02806b17f7
11 changed files with 193 additions and 616 deletions
|
@ -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:
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue