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

Merge pull request #3247 from iNavFlight/agh_home_heading_correction

Browse source 
Correct home yaw when a valid heading is acquired
This commit is contained in:
Alberto García Hierro 2018-06-06 12:33:18 +01:00 • committed by GitHub
commit f7c2824fc2
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
2 changed files with 70 additions and 13 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -777,7 +777,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
// If close to home - reset home position and land
if (posControl.homeDistance < navConfig()->general.min_rth_distance) {
setHomePosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
setHomePosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING, NAV_HOME_VALID_ALL);
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
return NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING
@ -1525,9 +1525,30 @@ void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, flo
*-----------------------------------------------------------*/
void updateActualHeading(bool headingValid, int32_t newHeading)
{
/* Update heading */
/* Update heading. Check if we're acquiring a valid heading for the
* first time and update home heading accordingly.
*/
navigationEstimateStatus_e newEstHeading = headingValid ? EST_TRUSTED : EST_NONE;
if (newEstHeading >= EST_USABLE && posControl.flags.estHeadingStatus < EST_USABLE &&
(posControl.homeFlags & (NAV_HOME_VALID_XY | NAV_HOME_VALID_Z)) &&
(posControl.homeFlags & NAV_HOME_VALID_HEADING) == 0) {
// Home was stored using the fake heading (assuming boot as 0deg). Calculate
// the offset from the fake to the actual yaw and apply the same rotation
// to the home point.
int32_t fakeToRealYawOffset = newHeading - posControl.actualState.yaw;
posControl.homePosition.yaw += fakeToRealYawOffset;
if (posControl.homePosition.yaw < 0) {
posControl.homePosition.yaw += DEGREES_TO_CENTIDEGREES(360);
}
if (posControl.homePosition.yaw >= DEGREES_TO_CENTIDEGREES(360)) {
posControl.homePosition.yaw -= DEGREES_TO_CENTIDEGREES(360);
}
posControl.homeFlags |= NAV_HOME_VALID_HEADING;
}
posControl.actualState.yaw = newHeading;
posControl.flags.estHeadingStatus = headingValid ? EST_TRUSTED : EST_NONE;
posControl.flags.estHeadingStatus = newEstHeading;
/* Precompute sin/cos of yaw angle */
posControl.actualState.sinYaw = sin_approx(CENTIDEGREES_TO_RADIANS(newHeading));
@ -1671,30 +1692,43 @@ bool validateRTHSanityChecker(void)
/*-----------------------------------------------------------
* Reset home position to current position
*-----------------------------------------------------------*/
void setHomePosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask)
void setHomePosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask, navigationHomeFlags_t homeFlags)
{
// XY-position
if ((useMask & NAV_POS_UPDATE_XY) != 0) {
posControl.homePosition.pos.x = pos->x;
posControl.homePosition.pos.y = pos->y;
if (homeFlags & NAV_HOME_VALID_XY) {
posControl.homeFlags |= NAV_HOME_VALID_XY;
} else {
posControl.homeFlags &= ~NAV_HOME_VALID_XY;
}
}
// Z-position
if ((useMask & NAV_POS_UPDATE_Z) != 0) {
posControl.homePosition.pos.z = pos->z;
if (homeFlags & NAV_HOME_VALID_Z) {
posControl.homeFlags |= NAV_HOME_VALID_Z;
} else {
posControl.homeFlags &= ~NAV_HOME_VALID_Z;
}
}
// Heading
if ((useMask & NAV_POS_UPDATE_HEADING) != 0) {
// Heading
posControl.homePosition.yaw = yaw;
if (homeFlags & NAV_HOME_VALID_HEADING) {
posControl.homeFlags |= NAV_HOME_VALID_HEADING;
} else {
posControl.homeFlags &= ~NAV_HOME_VALID_HEADING;
}
}
posControl.homeDistance = 0;
posControl.homeDirection = 0;
posControl.flags.isHomeValid = true;
// Update target RTH altitude as a waypoint above home
posControl.homeWaypointAbove = posControl.homePosition;
updateDesiredRTHAltitude();
@ -1703,6 +1737,21 @@ void setHomePosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t
ENABLE_STATE(GPS_FIX_HOME);
}
static navigationHomeFlags_t navigationActualStateHomeValidity(void)
{
navigationHomeFlags_t flags = 0;
if (posControl.flags.estPosStatus >= EST_USABLE) {
flags |= NAV_HOME_VALID_XY | NAV_HOME_VALID_Z;
}
if (posControl.flags.estHeadingStatus >= EST_USABLE) {
flags |= NAV_HOME_VALID_HEADING;
}
return flags;
}
/*-----------------------------------------------------------
* Update home position, calculate distance and bearing to home
*-----------------------------------------------------------*/
@ -1711,7 +1760,8 @@ void updateHomePosition(void)
// Disarmed and have a valid position, constantly update home
if (!ARMING_FLAG(ARMED)) {
if (posControl.flags.estPosStatus >= EST_USABLE) {
bool setHome = !posControl.flags.isHomeValid;
const navigationHomeFlags_t validHomeFlags = NAV_HOME_VALID_XY | NAV_HOME_VALID_Z;
bool setHome = (posControl.homeFlags & validHomeFlags) != validHomeFlags;
switch ((nav_reset_type_e)positionEstimationConfig()->reset_home_type) {
case NAV_RESET_NEVER:
break;
@ -1723,7 +1773,7 @@ void updateHomePosition(void)
break;
}
if (setHome) {
setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING );
setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
}
}
}
@ -1734,7 +1784,7 @@ void updateHomePosition(void)
if (IS_RC_MODE_ACTIVE(BOXHOMERESET)) {
if (isHomeResetAllowed && !FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && (posControl.flags.estPosStatus >= EST_USABLE)) {
const navSetWaypointFlags_t homeUpdateFlags = STATE(GPS_FIX_HOME) ? (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING) : (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, homeUpdateFlags);
setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, homeUpdateFlags, navigationActualStateHomeValidity());
isHomeResetAllowed = false;
}
}
@ -2029,7 +2079,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
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);
setHomePosition(&wpPos.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
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
// Only valid when armed and in poshold mode

13
src/main/navigation/navigation_private.h
View file

@ -62,6 +62,14 @@ typedef enum {
EST_TRUSTED = 2 // Estimate is usable and based on actual sensor data
} navigationEstimateStatus_e;
typedef enum {
NAV_HOME_INVALID = 0,
NAV_HOME_VALID_XY = 1 << 0,
NAV_HOME_VALID_Z = 1 << 1,
NAV_HOME_VALID_HEADING = 1 << 2,
NAV_HOME_VALID_ALL = NAV_HOME_VALID_XY | NAV_HOME_VALID_Z | NAV_HOME_VALID_HEADING,
} navigationHomeFlags_t;
typedef struct navigationFlags_s {
bool horizontalPositionDataNew;
bool verticalPositionDataNew;
@ -85,8 +93,6 @@ typedef struct navigationFlags_s {
bool isTerrainFollowEnabled; // Does iNav use rangefinder for terrain following (adjusting baro altitude target according to rangefinders readings)
bool forcedRTHActivated;
bool isHomeValid;
} navigationFlags_t;
typedef struct {
@ -326,6 +332,7 @@ typedef struct {
rthSanityChecker_t rthSanityChecker;
navWaypointPosition_t homePosition; // Special waypoint, stores original yaw (heading when launched)
navWaypointPosition_t homeWaypointAbove; // NEU-coordinates and initial bearing + desired RTH altitude
navigationHomeFlags_t homeFlags;
uint32_t homeDistance; // cm
int32_t homeDirection; // deg*100
@ -362,7 +369,7 @@ bool isLandingDetected(void);
navigationFSMStateFlags_t navGetCurrentStateFlags(void);
void setHomePosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask);
void setHomePosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask, navigationHomeFlags_t homeFlags);
void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask);
void setDesiredSurfaceOffset(float surfaceOffset);
void setDesiredPositionToFarAwayTarget(int32_t yaw, int32_t distance, navSetWaypointFlags_t useMask);