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 MSP for setting and getting safe homes (#6420)

Browse source
This commit is contained in:
Jonathan Hudson 2020-12-15 11:33:29 +00:00 • committed by GitHub
parent 846a77599a
commit fbc85d535d
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
2 changed files with 35 additions and 1 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1515,6 +1515,20 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
return true;
}
static mspResult_e mspFcSafeHomeOutCommand(sbuf_t *dst, sbuf_t *src)
{
const uint8_t safe_home_no = sbufReadU8(src); // get the home number
if(safe_home_no < MAX_SAFE_HOMES) {
sbufWriteU8(dst, safe_home_no);
sbufWriteU8(dst, safeHomeConfig(safe_home_no)->enabled);
sbufWriteU32(dst, safeHomeConfig(safe_home_no)->lat);
sbufWriteU32(dst, safeHomeConfig(safe_home_no)->lon);
return MSP_RESULT_ACK;
} else {
return MSP_RESULT_ERROR;
}
}
#ifdef USE_NAV
static void mspFcWaypointOutCommand(sbuf_t *dst, sbuf_t *src)
{
@ -2095,7 +2109,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
sbufReadU16(src); //Legacy yaw_jump_prevention_limit
gyroConfigMutable()->gyro_lpf = sbufReadU8(src);
accelerometerConfigMutable()->acc_lpf_hz = sbufReadU8(src);
sbufReadU8(src); //reserved
sbufReadU8(src); //reserved
sbufReadU8(src); //reserved
sbufReadU8(src); //reserved
sbufReadU8(src); //reserved
@ -2892,6 +2906,19 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
return MSP_RESULT_ERROR; // will only be reached if the rollback is not ready
break;
#endif
case MSP2_INAV_SET_SAFEHOME:
if (dataSize == 10) {
uint8_t i;
if (!sbufReadU8Safe(&i, src) || i >= MAX_SAFE_HOMES) {
return MSP_RESULT_ERROR;
}
safeHomeConfigMutable(i)->enabled = sbufReadU8(src);
safeHomeConfigMutable(i)->lat = sbufReadU32(src);
safeHomeConfigMutable(i)->lon = sbufReadU32(src);
} else {
return MSP_RESULT_ERROR;
}
break;
default:
return MSP_RESULT_ERROR;
@ -3195,6 +3222,10 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
break;
#endif
case MSP2_INAV_SAFEHOME:
*ret = mspFcSafeHomeOutCommand(dst, src);
break;
default:
// Not handled
return false;