Merge pull request #3183 from iNavFlight/agh_home_reset

Add new parameter for controlling home position reset
iNavFlight · May 9, 2018 · 0dce190 · 0dce190
2 parents 8a84949 + 954ae18
commit 0dce190
Show file tree

Hide file tree

Showing 5 changed files with 38 additions and 16 deletions.
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
@@ -47,7 +47,7 @@ tables:
  - name: gps_dyn_model
  values: ["PEDESTRIAN", "AIR_1G", "AIR_4G"]
  enum: gpsDynModel_e
- - name: reset_altitude
+ - name: reset_type
  values: ["NEVER", "FIRST_ARM", "EACH_ARM"]
  - name: nav_user_control_mode
  values: ["ATTI", "CRUISE"]
@@ -1069,7 +1069,10 @@ groups:
  type: bool
  - name: inav_reset_altitude
  field: reset_altitude_type
- table: reset_altitude
+ table: reset_type
+ - name: inav_reset_home
+ field: reset_home_type
+ table: reset_type
  - name: inav_max_surface_altitude
  field: max_surface_altitude
  min: 0

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
@@ -1714,6 +1714,8 @@ void setHomePosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t
  posControl.homeDistance = 0;
  posControl.homeDirection = 0;
 
+ posControl.flags.isHomeValid = true;
+
  // Update target RTH altitude as a waypoint above home
  posControl.homeWaypointAbove = posControl.homePosition;
  updateDesiredRTHAltitude();
@@ -1729,8 +1731,21 @@ void updateHomePosition(void)
 {
  // Disarmed and have a valid position, constantly update home
  if (!ARMING_FLAG(ARMED)) {
- if ((posControl.flags.estPosStatue >= EST_USABLE)) {
- setHomePosition(&posControl.actualState.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING );
+ if (posControl.flags.estPosStatue >= EST_USABLE) {
+ bool setHome = !posControl.flags.isHomeValid;
+ switch ((nav_reset_type_e)positionEstimationConfig()->reset_home_type) {
+ case NAV_RESET_NEVER:
+ break;
+ case NAV_RESET_ON_FIRST_ARM:
+ setHome |= !ARMING_FLAG(WAS_EVER_ARMED);
+ break;
+ case NAV_RESET_ON_EACH_ARM:
+ setHome = true;
+ break;
+ }
+ if (setHome) {
+ setHomePosition(&posControl.actualState.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
+ }
  }
  }
  else {

diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
@@ -62,11 +62,11 @@ enum {
  NAV_HEADING_CONTROL_MANUAL
 };
 
-enum {
- NAV_RESET_ALTITUDE_NEVER = 0,
- NAV_RESET_ALTITUDE_ON_FIRST_ARM,
- NAV_RESET_ALTITUDE_ON_EACH_ARM,
-};
+typedef enum {
+ NAV_RESET_NEVER = 0,
+ NAV_RESET_ON_FIRST_ARM,
+ NAV_RESET_ON_EACH_ARM,
+} nav_reset_type_e;
 
 typedef enum {
  NAV_RTH_ALLOW_LANDING_NEVER = 0,
@@ -76,7 +76,8 @@ typedef enum {
 
 typedef struct positionEstimationConfig_s {
  uint8_t automatic_mag_declination;
- uint8_t reset_altitude_type;
+ uint8_t reset_altitude_type; // from nav_reset_type_e
+ uint8_t reset_home_type; // nav_reset_type_e
  uint8_t gravity_calibration_tolerance; // Tolerance of gravity calibration (cm/s/s)
  uint8_t use_gps_velned;
 

diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c
@@ -177,12 +177,13 @@ typedef struct {
 
 static navigationPosEstimator_t posEstimator;
 
-PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 2);
+PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 3);
 
 PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
  // Inertial position estimator parameters
  .automatic_mag_declination = 1,
- .reset_altitude_type = NAV_RESET_ALTITUDE_ON_FIRST_ARM,
+ .reset_altitude_type = NAV_RESET_ON_FIRST_ARM,
+ .reset_home_type = NAV_RESET_ON_EACH_ARM,
  .gravity_calibration_tolerance = 5, // 5 cm/s/s calibration error accepted (0.5% of gravity)
  .use_gps_velned = 1, // "Disabled" is mandatory with gps_dyn_model = Pedestrian
 
@@ -243,12 +244,12 @@ static bool updateTimer(navigationTimer_t * tim, timeUs_t interval, timeUs_t cur
 
 static bool shouldResetReferenceAltitude(void)
 {
- switch (positionEstimationConfig()->reset_altitude_type) {
- case NAV_RESET_ALTITUDE_NEVER:
+ switch ((nav_reset_type_e)positionEstimationConfig()->reset_altitude_type) {
+ case NAV_RESET_NEVER:
  return false;
- case NAV_RESET_ALTITUDE_ON_FIRST_ARM:
+ case NAV_RESET_ON_FIRST_ARM:
  return !ARMING_FLAG(ARMED) && !ARMING_FLAG(WAS_EVER_ARMED);
- case NAV_RESET_ALTITUDE_ON_EACH_ARM:
+ case NAV_RESET_ON_EACH_ARM:
  return !ARMING_FLAG(ARMED);
  }
 

diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
@@ -85,6 +85,8 @@ 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 {