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

Make setHomePosition() accept a new argument indicating the home flags

Browse source 
Callers can specify wether XY, Z and HEADING are valid for the new
home position.
This commit is contained in:
Alberto García Hierro 2018-05-22 11:01:34 +01:00
parent 3e0ae8b23d
commit 200a85a2ca
2 changed files with 33 additions and 9 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -752,7 +752,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
// If close to home - reset home position and land // If close to home - reset home position and land
if (posControl.homeDistance < navConfig()->general.min_rth_distance) { 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); 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 return NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING
@ -1660,26 +1660,34 @@ bool validateRTHSanityChecker(void)
/*----------------------------------------------------------- /*-----------------------------------------------------------
* Reset home position to current position * 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 // XY-position
if ((useMask & NAV_POS_UPDATE_XY) != 0) { if ((useMask & NAV_POS_UPDATE_XY) != 0) {
posControl.homePosition.pos.x = pos->x; posControl.homePosition.pos.x = pos->x;
posControl.homePosition.pos.y = pos->y; posControl.homePosition.pos.y = pos->y;
posControl.homeFlags |= NAV_HOME_VALID_XY; if (homeFlags & NAV_HOME_VALID_XY) {
posControl.homeFlags |= NAV_HOME_VALID_XY;
} else {
posControl.homeFlags &= ~NAV_HOME_VALID_XY;
}
} }
// Z-position // Z-position
if ((useMask & NAV_POS_UPDATE_Z) != 0) { if ((useMask & NAV_POS_UPDATE_Z) != 0) {
posControl.homePosition.pos.z = pos->z; posControl.homePosition.pos.z = pos->z;
posControl.homeFlags |= NAV_HOME_VALID_Z; if (homeFlags & NAV_HOME_VALID_Z) {
posControl.homeFlags |= NAV_HOME_VALID_Z;
} else {
posControl.homeFlags &= ~NAV_HOME_VALID_Z;
}
} }
// Heading // Heading
if ((useMask & NAV_POS_UPDATE_HEADING) != 0) { if ((useMask & NAV_POS_UPDATE_HEADING) != 0) {
// Heading // Heading
posControl.homePosition.yaw = yaw; posControl.homePosition.yaw = yaw;
if (posControl.flags.estHeadingStatus >= EST_USABLE) { if (homeFlags & NAV_HOME_VALID_HEADING) {
posControl.homeFlags |= NAV_HOME_VALID_HEADING; posControl.homeFlags |= NAV_HOME_VALID_HEADING;
} else { } else {
posControl.homeFlags &= ~NAV_HOME_VALID_HEADING; posControl.homeFlags &= ~NAV_HOME_VALID_HEADING;
@ -1697,6 +1705,21 @@ void setHomePosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t
ENABLE_STATE(GPS_FIX_HOME); 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 * Update home position, calculate distance and bearing to home
*-----------------------------------------------------------*/ *-----------------------------------------------------------*/
@ -1718,7 +1741,7 @@ void updateHomePosition(void)
break; break;
} }
if (setHome) { 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());
} }
} }
} }
@ -1729,7 +1752,7 @@ void updateHomePosition(void)
if (IS_RC_MODE_ACTIVE(BOXHOMERESET)) { 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)) { 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); 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; isHomeResetAllowed = false;
} }
} }
@ -2024,7 +2047,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) { 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 // 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(&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 // WP #255 - special waypoint - directly set desiredPosition
// Only valid when armed and in poshold mode // Only valid when armed and in poshold mode

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

@ -67,6 +67,7 @@ typedef enum {
NAV_HOME_VALID_XY = 1 << 0, NAV_HOME_VALID_XY = 1 << 0,
NAV_HOME_VALID_Z = 1 << 1, NAV_HOME_VALID_Z = 1 << 1,
NAV_HOME_VALID_HEADING = 1 << 2, NAV_HOME_VALID_HEADING = 1 << 2,
NAV_HOME_VALID_ALL = NAV_HOME_VALID_XY | NAV_HOME_VALID_Z | NAV_HOME_VALID_HEADING,
} navigationHomeFlags_t; } navigationHomeFlags_t;
typedef struct navigationFlags_s { typedef struct navigationFlags_s {
@ -325,7 +326,7 @@ bool isLandingDetected(void);
navigationFSMStateFlags_t navGetCurrentStateFlags(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 setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask);
void setDesiredSurfaceOffset(float surfaceOffset); void setDesiredSurfaceOffset(float surfaceOffset);
void setDesiredPositionToFarAwayTarget(int32_t yaw, int32_t distance, navSetWaypointFlags_t useMask); void setDesiredPositionToFarAwayTarget(int32_t yaw, int32_t distance, navSetWaypointFlags_t useMask);