Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-20 14:55:21 +03:00
Code Issues Wiki Activity

Split MSP_WP command into separate function

Browse source
This commit is contained in:
Martin Budden 2016-10-18 21:49:31 +01:00
parent 60e80c23d7
commit 613239b8c8
2 changed files with 29 additions and 22 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -518,10 +518,6 @@ static uint32_t packFlightModeFlags(void)
static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn)
{
uint32_t i;
#ifdef GPS
uint8_t wp_no;
int32_t lat = 0, lon = 0;
#endif
switch (cmdMSP) {
case MSP_API_VERSION:
@ -803,23 +799,6 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
sbufWriteU16(dst, GPS_directionToHome);
sbufWriteU8(dst, GPS_update & 1);
break;
case MSP_WP:
wp_no = sbufReadU8(src); // get the wp number
if (wp_no == 0) {
lat = GPS_home[LAT];
lon = GPS_home[LON];
} else if (wp_no == 16) {
lat = GPS_hold[LAT];
lon = GPS_hold[LON];
}
sbufWriteU8(dst, wp_no);
sbufWriteU32(dst, lat);
sbufWriteU32(dst, lon);
sbufWriteU32(dst, AltHold); // altitude (cm) will come here -- temporary implementation to test feature with apps
sbufWriteU16(dst, 0); // heading will come here (deg)
sbufWriteU16(dst, 0); // time to stay (ms) will come here
sbufWriteU8(dst, 0); // nav flag will come here
break;
case MSP_GPSSVINFO:
sbufWriteU8(dst, GPS_numCh);
for (i = 0; i < GPS_numCh; i++){
@ -1126,6 +1105,29 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
return true;
}
#ifdef GPS
static void mspFcWpCommand(sbuf_t *dst, sbuf_t *src)
{
uint8_t wp_no;
int32_t lat = 0, lon = 0;
wp_no = sbufReadU8(src); // get the wp number
if (wp_no == 0) {
lat = GPS_home[LAT];
lon = GPS_home[LON];
} else if (wp_no == 16) {
lat = GPS_hold[LAT];
lon = GPS_hold[LON];
}
sbufWriteU8(dst, wp_no);
sbufWriteU32(dst, lat);
sbufWriteU32(dst, lon);
sbufWriteU32(dst, AltHold); // altitude (cm) will come here -- temporary implementation to test feature with apps
sbufWriteU16(dst, 0); // heading will come here (deg)
sbufWriteU16(dst, 0); // time to stay (ms) will come here
sbufWriteU8(dst, 0); // nav flag will come here
}
#endif
#ifdef USE_FLASHFS
static void mspFcDataFlashReadCommand(sbuf_t *dst, sbuf_t *src)
{
@ -1752,6 +1754,11 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro
if (mspFcProcessOutCommand(cmdMSP, dst, mspPostProcessFn)) {
ret = MSP_RESULT_ACK;
#ifdef GPS
} else if (cmdMSP == MSP_WP) {
mspFcWpCommand(dst, src);
ret = MSP_RESULT_ACK;
#endif
#ifdef USE_FLASHFS
} else if (cmdMSP == MSP_DATAFLASH_READ) {
mspFcDataFlashReadCommand(dst, src);

2
src/main/msp/msp_serial.h
View file

@ -37,7 +37,7 @@ typedef enum {
MSP_SKIP_NON_MSP_DATA
} mspEvaluateNonMspData_e;
#define MSP_PORT_INBUF_SIZE 256
#define MSP_PORT_INBUF_SIZE 192
#ifdef USE_FLASHFS
#define MSP_PORT_OUTBUF_SIZE (4096 + 16)
#else