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

WP mission sealevel altitude datum (#6662)

Browse source 
Changed to using WP P3 for altitude datum flag now set individually for each geospatial WP in mission.
This commit is contained in:
breadoven 2021-05-03 14:44:56 +01:00 • committed by GitHub
parent 602ce538ba
commit f4b92bc0f7
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
3 changed files with 21 additions and 8 deletions
Download patch file Download diff file Expand all files Collapse all files

5
src/main/io/osd.c
View file

@ -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);
}
}
}

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

@ -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;

7
src/main/navigation/navigation.h
View file

@ -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);