From f4b92bc0f766c88c8b2f3ff5faad85181b924b04 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 3 May 2021 14:44:56 +0100 Subject: [PATCH] WP mission sealevel altitude datum (#6662) Changed to using WP P3 for altitude datum flag now set individually for each geospatial WP in mission. --- src/main/io/osd.c | 5 +++-- src/main/navigation/navigation.c | 17 +++++++++++------ src/main/navigation/navigation.h | 7 +++++++ 3 files changed, 21 insertions(+), 8 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 7f9d038753..81ab58b480 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1963,9 +1963,10 @@ static bool osdDrawSingleElement(uint8_t item) wp2.lon = posControl.waypointList[j].lon; wp2.alt = posControl.waypointList[j].alt; fpVector3_t poi; - geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp2, GEO_ALT_RELATIVE); + geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp2, waypointMissionAltConvMode(posControl.waypointList[j].p3)); + int32_t altConvModeAltitude = waypointMissionAltConvMode(posControl.waypointList[j].p3) == GEO_ALT_ABSOLUTE ? osdGetAltitudeMsl() : osdGetAltitude(); while (j > 9) j -= 10; // Only the last digit displayed if WP>=10, no room for more - osdHudDrawPoi(calculateDistanceToDestination(&poi) / 100, osdGetHeadingAngle(calculateBearingToDestination(&poi) / 100), (posControl.waypointList[j].alt - osdGetAltitude())/ 100, 2, SYM_WAYPOINT, 49 + j, i); + osdHudDrawPoi(calculateDistanceToDestination(&poi) / 100, osdGetHeadingAngle(calculateBearingToDestination(&poi) / 100), (posControl.waypointList[j].alt - altConvModeAltitude)/ 100, 2, SYM_WAYPOINT, 49 + j, i); } } } diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 94db2be867..94e3c85e0f 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -236,7 +236,7 @@ void calculateInitialHoldPosition(fpVector3_t * pos); void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t distance); void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distance); static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWaypointHome); -static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint); +static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv); static navigationFSMEvent_t nextForNonGeoStates(void); static bool isWaypointMissionValid(void); @@ -1502,7 +1502,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav if (STATE(MULTIROTOR)) { wpHeadingControl.mode = NAV_WP_HEAD_MODE_POI; mapWaypointToLocalPosition(&wpHeadingControl.poi_pos, - &posControl.waypointList[posControl.activeWaypointIndex]); + &posControl.waypointList[posControl.activeWaypointIndex], GEO_ALT_RELATIVE); } return nextForNonGeoStates(); @@ -2890,7 +2890,7 @@ void resetSafeHomes(void) } #endif -static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint) +static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv) { gpsLocation_t wpLLH; @@ -2898,7 +2898,7 @@ static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint wpLLH.lon = waypoint->lon; wpLLH.alt = waypoint->alt; - geoConvertGeodeticToLocal(localPos, &posControl.gpsOrigin, &wpLLH, GEO_ALT_RELATIVE); + geoConvertGeodeticToLocal(localPos, &posControl.gpsOrigin, &wpLLH, altConv); } static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos) @@ -2912,10 +2912,15 @@ static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos setDesiredPosition(&posControl.activeWaypoint.pos, posControl.activeWaypoint.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); } +geoAltitudeConversionMode_e waypointMissionAltConvMode(geoAltitudeDatumFlag_e datumFlag) +{ + return datumFlag == NAV_WP_MSL_DATUM ? GEO_ALT_ABSOLUTE : GEO_ALT_RELATIVE; +} + static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint) { fpVector3_t localPos; - mapWaypointToLocalPosition(&localPos, waypoint); + mapWaypointToLocalPosition(&localPos, waypoint, waypointMissionAltConvMode(waypoint->p3)); calculateAndSetActiveWaypointToLocalPosition(&localPos); } @@ -3321,7 +3326,7 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass) // Don't allow arming if first waypoint is farther than configured safe distance if ((posControl.waypointCount > 0) && (navConfig()->general.waypoint_safe_distance != 0)) { fpVector3_t startingWaypointPos; - mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[0]); + mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[0], GEO_ALT_RELATIVE); const bool navWpMissionStartTooFar = calculateDistanceToDestination(&startingWaypointPos) > navConfig()->general.waypoint_safe_distance; diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 0638e6e844..86cec421bf 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -481,6 +481,11 @@ typedef enum { GEO_ORIGIN_RESET_ALTITUDE } geoOriginResetMode_e; +typedef enum { + NAV_WP_TAKEOFF_DATUM, + NAV_WP_MSL_DATUM +} geoAltitudeDatumFlag_e; + // geoSetOrigin stores the location provided in llh as a GPS origin in the // provided origin parameter. resetMode indicates wether all origin coordinates // should be overwritten by llh (GEO_ORIGIN_SET) or just the altitude, leaving @@ -501,6 +506,8 @@ bool geoConvertGeodeticToLocalOrigin(fpVector3_t * pos, const gpsLocation_t *llh // the provided origin is valid and the conversion could be performed. bool geoConvertLocalToGeodetic(gpsLocation_t *llh, const gpsOrigin_t *origin, const fpVector3_t *pos); float geoCalculateMagDeclination(const gpsLocation_t * llh); // degrees units +// Select absolute or relative altitude based on WP mission flag setting +geoAltitudeConversionMode_e waypointMissionAltConvMode(geoAltitudeDatumFlag_e datumFlag); /* Distance/bearing calculation */ bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos);