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

Merge pull request #1229 from iNavFlight/nav-improvements

Browse source 
Navigation and GPS improvements
This commit is contained in:
Konstantin Sharlaimov 2017-02-05 21:07:23 +10:00 • committed by GitHub
commit dcc163f185
8 changed files with 82 additions and 30 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -80,6 +80,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
// General navigation parameters
.pos_failure_timeout = 5, // 5 sec
.waypoint_radius = 100, // 2m diameter
.waypoint_safe_distance = 10000, // 100m - first waypoint should be closer than this
.max_speed = 300, // 3 m/s = 10.8 km/h
.max_climb_rate = 500, // 5 m/s
.max_manual_speed = 500,
@ -2213,6 +2214,17 @@ void resetWaypointList(void)
}
}
static void mapWaypointToLocalPosition(t_fp_vector * localPos, const navWaypoint_t * waypoint)
{
gpsLocation_t wpLLH;
wpLLH.lat = waypoint->lat;
wpLLH.lon = waypoint->lon;
wpLLH.alt = waypoint->alt;
geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, localPos, GEO_ALT_RELATIVE);
}
static void calcualteAndSetActiveWaypointToLocalPosition(const t_fp_vector * pos)
{
posControl.activeWaypoint.pos = *pos;
@ -2226,14 +2238,8 @@ static void calcualteAndSetActiveWaypointToLocalPosition(const t_fp_vector * pos
static void calcualteAndSetActiveWaypoint(const navWaypoint_t * waypoint)
{
gpsLocation_t wpLLH;
t_fp_vector localPos;
wpLLH.lat = waypoint->lat;
wpLLH.lon = waypoint->lon;
wpLLH.alt = waypoint->alt;
geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &localPos, GEO_ALT_RELATIVE);
mapWaypointToLocalPosition(&localPos, waypoint);
calcualteAndSetActiveWaypointToLocalPosition(&localPos);
}
@ -2333,7 +2339,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
if (posControl.flags.hasValidAltitudeSensor) navFlags |= (1 << 0);
if (posControl.flags.hasValidSurfaceSensor) navFlags |= (1 << 1);
if (posControl.flags.hasValidPositionSensor) navFlags |= (1 << 2);
if ((STATE(GPS_FIX) && gpsSol.numSat >= positionEstimationConfig()->gps_min_sats)) navFlags |= (1 << 3);
//if (STATE(GPS_FIX)) navFlags |= (1 << 3);
#if defined(NAV_GPS_GLITCH_DETECTION)
if (isGPSGlitchDetected()) navFlags |= (1 << 4);
#endif
@ -2536,6 +2542,18 @@ bool naivationBlockArming(void)
shouldBlockArming = true;
}
// Don't allow arming if first waypoint is farther than configured safe distance
if (posControl.waypointCount > 0) {
t_fp_vector startingWaypointPos;
mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[0]);
const bool navWpMissionStartTooFar = calculateDistanceToDestination(&startingWaypointPos) > navConfig()->general.waypoint_safe_distance;
if (navWpMissionStartTooFar) {
shouldBlockArming = true;
}
}
return shouldBlockArming;
}