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

Fix MSP

Browse source
This commit is contained in:
breadoven 2022-08-24 15:44:19 +01:00
parent 7322fbedc8
commit 0598080dc7
2 changed files with 11 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1580,7 +1580,7 @@ static void mspFcWaypointOutCommand(sbuf_t *dst, sbuf_t *src)
{ {
const uint8_t msp_wp_no = sbufReadU8(src); // get the wp number const uint8_t msp_wp_no = sbufReadU8(src); // get the wp number
navWaypoint_t msp_wp; navWaypoint_t msp_wp;
getWaypoint(msp_wp_no + wpMissionStartIndex, &msp_wp); getWaypoint(msp_wp_no, &msp_wp);
sbufWriteU8(dst, msp_wp_no); // wp_no sbufWriteU8(dst, msp_wp_no); // wp_no
sbufWriteU8(dst, msp_wp.action); // action (WAYPOINT) sbufWriteU8(dst, msp_wp.action); // action (WAYPOINT)
sbufWriteU32(dst, msp_wp.lat); // lat sbufWriteU32(dst, msp_wp.lat); // lat

4
src/main/navigation/navigation.c
View file

@ -3072,8 +3072,8 @@ void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData)
} }
// WP #1 - #60 - common waypoints - pre-programmed mission // WP #1 - #60 - common waypoints - pre-programmed mission
else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS)) { else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS)) {
if (wpNumber <= posControl.waypointCount + wpMissionStartIndex) { if (wpNumber <= posControl.waypointCount) {
*wpData = posControl.waypointList[wpNumber - 1]; *wpData = posControl.waypointList[wpNumber + wpMissionStartIndex - 1];
if(wpData->action == NAV_WP_ACTION_JUMP) { if(wpData->action == NAV_WP_ACTION_JUMP) {
wpData->p1 += 1; // make WP # (vice index) wpData->p1 += 1; // make WP # (vice index)
} }