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

Callers can specify wether XY, Z and HEADING are valid for the new home position.
shellixyz · May 22, 2018 · 200a85a · 200a85a
1 parent 3e0ae8b
commit 200a85a
Show file tree

Hide file tree

Showing 2 changed files with 33 additions and 9 deletions.
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
@@ -752,7 +752,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
@@ -1660,26 +1660,34 @@ 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;
- 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
  if ((useMask & NAV_POS_UPDATE_Z) != 0) {
  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
  if ((useMask & NAV_POS_UPDATE_HEADING) != 0) {
  // Heading
  posControl.homePosition.yaw = yaw;
- if (posControl.flags.estHeadingStatus >= EST_USABLE) {
+ if (homeFlags & NAV_HOME_VALID_HEADING) {
  posControl.homeFlags |= NAV_HOME_VALID_HEADING;
  } else {
  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);
 }
 
+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
  *-----------------------------------------------------------*/
@@ -1718,7 +1741,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());
  }
  }
  }
@@ -1729,7 +1752,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;
  }
  }
@@ -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) {
  // 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

diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
@@ -67,6 +67,7 @@ typedef enum {
  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 {
@@ -325,7 +326,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);