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

Cleanup geo* functions

Browse source 
- Rename gpsOrigin_s type to gpsOrigin_t
- Make geo* conversion functions return wether they succeeded or not
- Reorder geo* functions arguments to follow the (output, inputs)
convention
- Document all the geo* functions
This commit is contained in:
Alberto García Hierro 2018-12-26 11:56:37 +00:00
parent 38c0caa12b
commit c750b5dfd2
5 changed files with 42 additions and 19 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1943,7 +1943,7 @@ bool isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWayp
static void updateHomePositionCompatibility(void)
{
geoConvertLocalToGeodetic(&posControl.gpsOrigin, &posControl.homePosition.pos, &GPS_home);
geoConvertLocalToGeodetic(&GPS_home, &posControl.gpsOrigin, &posControl.homePosition.pos);
GPS_distanceToHome = posControl.homeDistance / 100;
GPS_directionToHome = posControl.homeDirection / 100;
}
@ -2480,7 +2480,7 @@ void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData)
else if (wpNumber == 255) {
gpsLocation_t wpLLH;
geoConvertLocalToGeodetic(&posControl.gpsOrigin, &navGetCurrentActualPositionAndVelocity()->pos, &wpLLH);
geoConvertLocalToGeodetic(&wpLLH, &posControl.gpsOrigin, &navGetCurrentActualPositionAndVelocity()->pos);
wpData->lat = wpLLH.lat;
wpData->lon = wpLLH.lon;
@ -2507,7 +2507,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
// WP #0 - special waypoint - HOME
if ((wpNumber == 0) && ARMING_FLAG(ARMED) && (posControl.flags.estPosStatus >= EST_USABLE) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled) {
// Forcibly set home position. Note that this is only valid if already armed, otherwise home will be reset instantly
geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &wpPos.pos, GEO_ALT_RELATIVE);
geoConvertGeodeticToLocal(&wpPos.pos, &posControl.gpsOrigin, &wpLLH, GEO_ALT_RELATIVE);
setHomePosition(&wpPos.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, NAV_HOME_VALID_ALL);
}
// WP #255 - special waypoint - directly set desiredPosition
@ -2516,7 +2516,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
ARMING_FLAG(ARMED) && (posControl.flags.estPosStatus == EST_TRUSTED) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled &&
(posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)) {
// Convert to local coordinates
geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &wpPos.pos, GEO_ALT_RELATIVE);
geoConvertGeodeticToLocal(&wpPos.pos, &posControl.gpsOrigin, &wpLLH, GEO_ALT_RELATIVE);
navSetWaypointFlags_t waypointUpdateFlags = NAV_POS_UPDATE_XY;
@ -2611,7 +2611,7 @@ static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint
wpLLH.lon = waypoint->lon;
wpLLH.alt = waypoint->alt;
geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, localPos, GEO_ALT_RELATIVE);
geoConvertGeodeticToLocal(localPos, &posControl.gpsOrigin, &wpLLH, GEO_ALT_RELATIVE);
}
static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos)