Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-21 15:25:29 +03:00
Code Issues Wiki Activity

add new special waypoint 254 to facilitate desiredPosition in GCS assisted mode

Browse source
This commit is contained in:
Yuxin 2020-09-02 22:27:56 +08:00
parent b8db639a2a
commit 2a76ce302c
1 changed files with 14 additions and 7 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -2782,21 +2782,28 @@ void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData)
wpData->alt = GPS_home.alt;
}
}
// WP #255 - special waypoint - get desiredPosition that was set by ground control station if in GCS assisted mode, otherwise return current position
// WP #255 - special waypoint - directly get actualPosition
else if (wpNumber == 255) {
gpsLocation_t wpLLH;
if ((posControl.gpsOrigin.valid) && (posControl.flags.isGCSAssistedNavigationEnabled) && (posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)) {
geoConvertLocalToGeodetic(&wpLLH, &posControl.gpsOrigin, &posControl.desiredState.pos);
}
else {
geoConvertLocalToGeodetic(&wpLLH, &posControl.gpsOrigin, &navGetCurrentActualPositionAndVelocity()->pos);
}
wpData->lat = wpLLH.lat;
wpData->lon = wpLLH.lon;
wpData->alt = wpLLH.alt;
}
// WP #254 - special waypoint - get desiredPosition that was set by ground control station if in GCS assisted mode
else if (wpNumber == 254) {
if ((posControl.gpsOrigin.valid) && (posControl.flags.isGCSAssistedNavigationEnabled) && (posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)) {
gpsLocation_t wpLLH;
geoConvertLocalToGeodetic(&wpLLH, &posControl.gpsOrigin, &posControl.desiredState.pos);
wpData->lat = wpLLH.lat;
wpData->lon = wpLLH.lon;
wpData->alt = wpLLH.alt;
}
}
// WP #1 - #60 - common waypoints - pre-programmed mission
else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS)) {
if (wpNumber <= posControl.waypointCount) {