mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-22 15:55:40 +03:00
WP mission sealevel altitude datum (#6662)
Changed to using WP P3 for altitude datum flag now set individually for each geospatial WP in mission.
This commit is contained in:
parent
602ce538ba
commit
f4b92bc0f7
3 changed files with 21 additions and 8 deletions
|
@ -1963,9 +1963,10 @@ static bool osdDrawSingleElement(uint8_t item)
|
||||||
wp2.lon = posControl.waypointList[j].lon;
|
wp2.lon = posControl.waypointList[j].lon;
|
||||||
wp2.alt = posControl.waypointList[j].alt;
|
wp2.alt = posControl.waypointList[j].alt;
|
||||||
fpVector3_t poi;
|
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
|
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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -236,7 +236,7 @@ void calculateInitialHoldPosition(fpVector3_t * pos);
|
||||||
void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t distance);
|
void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t distance);
|
||||||
void calculateNewCruiseTarget(fpVector3_t * origin, 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 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 navigationFSMEvent_t nextForNonGeoStates(void);
|
||||||
static bool isWaypointMissionValid(void);
|
static bool isWaypointMissionValid(void);
|
||||||
|
|
||||||
|
@ -1502,7 +1502,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
|
||||||
if (STATE(MULTIROTOR)) {
|
if (STATE(MULTIROTOR)) {
|
||||||
wpHeadingControl.mode = NAV_WP_HEAD_MODE_POI;
|
wpHeadingControl.mode = NAV_WP_HEAD_MODE_POI;
|
||||||
mapWaypointToLocalPosition(&wpHeadingControl.poi_pos,
|
mapWaypointToLocalPosition(&wpHeadingControl.poi_pos,
|
||||||
&posControl.waypointList[posControl.activeWaypointIndex]);
|
&posControl.waypointList[posControl.activeWaypointIndex], GEO_ALT_RELATIVE);
|
||||||
}
|
}
|
||||||
return nextForNonGeoStates();
|
return nextForNonGeoStates();
|
||||||
|
|
||||||
|
@ -2890,7 +2890,7 @@ void resetSafeHomes(void)
|
||||||
}
|
}
|
||||||
#endif
|
#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;
|
gpsLocation_t wpLLH;
|
||||||
|
|
||||||
|
@ -2898,7 +2898,7 @@ static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint
|
||||||
wpLLH.lon = waypoint->lon;
|
wpLLH.lon = waypoint->lon;
|
||||||
wpLLH.alt = waypoint->alt;
|
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)
|
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);
|
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)
|
static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint)
|
||||||
{
|
{
|
||||||
fpVector3_t localPos;
|
fpVector3_t localPos;
|
||||||
mapWaypointToLocalPosition(&localPos, waypoint);
|
mapWaypointToLocalPosition(&localPos, waypoint, waypointMissionAltConvMode(waypoint->p3));
|
||||||
calculateAndSetActiveWaypointToLocalPosition(&localPos);
|
calculateAndSetActiveWaypointToLocalPosition(&localPos);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -3321,7 +3326,7 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
|
||||||
// Don't allow arming if first waypoint is farther than configured safe distance
|
// Don't allow arming if first waypoint is farther than configured safe distance
|
||||||
if ((posControl.waypointCount > 0) && (navConfig()->general.waypoint_safe_distance != 0)) {
|
if ((posControl.waypointCount > 0) && (navConfig()->general.waypoint_safe_distance != 0)) {
|
||||||
fpVector3_t startingWaypointPos;
|
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;
|
const bool navWpMissionStartTooFar = calculateDistanceToDestination(&startingWaypointPos) > navConfig()->general.waypoint_safe_distance;
|
||||||
|
|
||||||
|
|
|
@ -481,6 +481,11 @@ typedef enum {
|
||||||
GEO_ORIGIN_RESET_ALTITUDE
|
GEO_ORIGIN_RESET_ALTITUDE
|
||||||
} geoOriginResetMode_e;
|
} 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
|
// geoSetOrigin stores the location provided in llh as a GPS origin in the
|
||||||
// provided origin parameter. resetMode indicates wether all origin coordinates
|
// provided origin parameter. resetMode indicates wether all origin coordinates
|
||||||
// should be overwritten by llh (GEO_ORIGIN_SET) or just the altitude, leaving
|
// 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.
|
// the provided origin is valid and the conversion could be performed.
|
||||||
bool geoConvertLocalToGeodetic(gpsLocation_t *llh, const gpsOrigin_t *origin, const fpVector3_t *pos);
|
bool geoConvertLocalToGeodetic(gpsLocation_t *llh, const gpsOrigin_t *origin, const fpVector3_t *pos);
|
||||||
float geoCalculateMagDeclination(const gpsLocation_t * llh); // degrees units
|
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 */
|
/* Distance/bearing calculation */
|
||||||
bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos);
|
bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos);
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue