Copter: home is set becomes three state

ArduCopter/AP_State.pde

@@ -1,18 +1,27 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-
+// set_home_state - update home state
-void set_home_is_set(bool b)
+void set_home_state(enum HomeState new_home_state)
 {
     // if no change, exit immediately
-    if( ap.home_is_set == b )
+    if (ap.home_state == new_home_state)
         return;
 
-    ap.home_is_set 	= b;
+    // update state
-    if(b) {
+    ap.home_state = new_home_state;
+
+    // log if home has been set
+    if (new_home_state == HOME_SET_NOT_LOCKED || new_home_state == HOME_SET_AND_LOCKED) {
         Log_Write_Event(DATA_SET_HOME);
     }
 }
 
+// home_is_set - returns true if home positions has been set (to GPS location, armed location or EKF origin)
+bool home_is_set()
+{
+    return (ap.home_state == HOME_SET_NOT_LOCKED || ap.home_state == HOME_SET_AND_LOCKED);
+}
+
 // ---------------------------------------------
 void set_auto_armed(bool b)
 {

ArduCopter/ArduCopter.pde

@@ -356,7 +356,7 @@ static bool sonar_enabled = true; // enable user switch for sonar
 //Documentation of GLobals:
 static union {
     struct {
-        uint8_t home_is_set         : 1; // 0
+        uint8_t unused1             : 1; // 0
         uint8_t simple_mode         : 2; // 1,2 // This is the state of simple mode : 0 = disabled ; 1 = SIMPLE ; 2 = SUPERSIMPLE
         uint8_t pre_arm_rc_check    : 1; // 3   // true if rc input pre-arm checks have been completed successfully
         uint8_t pre_arm_check       : 1; // 4   // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed
@@ -373,6 +373,9 @@ static union {
         uint8_t initialised         : 1; // 17  // true once the init_ardupilot function has completed.  Extended status to GCS is not sent until this completes
         uint8_t land_complete_maybe : 1; // 18  // true if we may have landed (less strict version of land_complete)
         uint8_t throttle_zero       : 1; // 19  // true if the throttle stick is at zero, debounced
+        uint8_t system_time_set     : 1; // 20  // true if the system time has been set from the GPS
+        uint8_t gps_base_pos_set    : 1; // 21  // true when the gps base position has been set (used for RTK gps only)
+        enum HomeState home_state   : 2; // 22,23 - home status (unset, set, locked)
     };
     uint32_t value;
 } ap;
@@ -1153,7 +1156,6 @@ static void one_hz_loop()
 static void update_GPS(void)
 {
     static uint32_t last_gps_reading[GPS_MAX_INSTANCES];   // time of last gps message
-    static uint8_t ground_start_count = 10;     // counter used to grab at least 10 reads before commiting the Home location
     bool report_gps_glitch;
     bool gps_updated = false;
 
@@ -1175,7 +1177,7 @@ static void update_GPS(void)
 
     if (gps_updated) {
         // run glitch protection and update AP_Notify if home has been initialised
-        if (ap.home_is_set) {
+        if (home_is_set()) {
             gps_glitch.check_position();
             report_gps_glitch = (gps_glitch.glitching() && !ap.usb_connected && hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED);
             if (AP_Notify::flags.gps_glitching != report_gps_glitch) {
@@ -1188,48 +1190,17 @@ static void update_GPS(void)
             }
         }
 
-        // checks to initialise home and take location based pictures
+        // set system time if necessary
-        if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
+        set_system_time_from_GPS();
 
-            // check if we can initialise home yet
+        // update home from GPS location if necessary
-            if (!ap.home_is_set) {
+        update_home_from_GPS();
-                // if we have a 3d lock and valid location
-                if(gps.status() >= AP_GPS::GPS_OK_FIX_3D && gps.location().lat != 0) {
-                    if (ground_start_count > 0 ) {
-                        ground_start_count--;
-                    } else {
-                        // after 10 successful reads store home location
-                        // ap.home_is_set will be true so this will only happen once
-                        ground_start_count = 0;
-                        init_home();
-
-                        // set system clock for log timestamps
-                        hal.util->set_system_clock(gps.time_epoch_usec());
-                        
-                        if (g.compass_enabled) {
-                            // Set compass declination automatically
-                            compass.set_initial_location(gps.location().lat, gps.location().lng);
-                        }
-                    }
-                } else {
-                    // start again if we lose 3d lock
-                    ground_start_count = 10;
-                }
-            } else {
-                // update home position when not armed
-                if (!motors.armed()) {
-                    update_home();
-                }
-            }
 
-            //If we are not currently armed, and we're ready to 
+        // check gps base position (used for RTK only)
-            //enter RTK mode, then capture current state as home,
+        check_gps_base_pos();
-            //and enter RTK fixes!
-            if (!motors.armed() && gps.can_calculate_base_pos()) {
 
-                gps.calculate_base_pos();
+        // checks to initialise home and take location based pictures
-
+        if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
-            }
 
 #if CAMERA == ENABLED
             if (camera.update_location(current_loc) == true) {

ArduCopter/commands.pde

@@ -1,17 +1,102 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-// run this at setup on the ground
+/*
-// -------------------------------
+ * the home_state has a number of possible values (see enum HomeState in defines.h's)
-static void init_home()
+ *   HOME_UNSET             = home is not set, no GPS positions yet received
+ *   HOME_SET_NOT_LOCKED    = home is set to EKF origin or armed location (can be moved)
+ *   HOME_SET_AND_LOCKED    = home has been set by user, cannot be moved except by user initiated do-set-home command
+ */
+
+static uint8_t ground_start_count = 10;     // counter used to grab at least 10 reads before accepting a GPS location as home location
+
+// checks if we should update ahrs/RTL home position from GPS
+static void update_home_from_GPS()
+{
+    // exit immediately if counter has run down and home already set
+    if (ground_start_count == 0 && ap.home_state != HOME_UNSET) {
+        return;
+    }
+
+    // if counter has not run down
+    if (ground_start_count > 0) {
+
+        // reset counter if we do not have GPS lock
+        if (gps.status() < AP_GPS::GPS_OK_FIX_3D) {
+            ground_start_count = 10;
+
+        // count down for 10 consecutive locks
+        } else {
+           ground_start_count--;
+        }
+
+        return;
+    }
+
+    // move home to current gps location (this will set home_state to HOME_SET)
+    set_home(gps.location());
+}
+
+// set_home_to_current_location - set home to current GPS location
+static bool set_home_to_current_location() {
+    // exit with failure if we haven't had 10 good GPS position
+    if (ground_start_count > 0) {
+        return false;
+    }
+
+    // set home to latest gps location
+    return set_home(gps.location());
+}
+
+// set_home_to_current_location_and_lock - set home to current location and lock so it cannot be moved
+static bool set_home_to_current_location_and_lock()
+{
+    if (set_home_to_current_location()) {
+        set_home_state(HOME_SET_AND_LOCKED);
+        return true;
+    }
+    return false;
+}
+
+// set_home_and_lock - sets ahrs home (used for RTL) to specified location and locks so it cannot be moved
+//  unless this function is called again
+static bool set_home_and_lock(const Location& loc)
 {
-    set_home_is_set(true);
+    if (set_home(loc)) {
+        set_home_state(HOME_SET_AND_LOCKED);
+        return true;
+    }
+    return false;
+}
 
-    // copter uses 0 home altitude
+// set_home - sets ahrs home (used for RTL) to specified location
-    Location loc = gps.location();
+//  initialises inertial nav and compass on first call
+//  returns true if home location set successfully
+static bool set_home(const Location& loc)
+{
+    // check location is valid
+    if (loc.lat == 0 && loc.lng == 0) {
+        return false;
+    }
 
+    // set ahrs home (used for RTL)
     ahrs.set_home(loc);
 
-    inertial_nav.setup_home_position();
+    // init inav and compass declination
+    if (ap.home_state == HOME_UNSET) {
+        // Set compass declination automatically
+        if (g.compass_enabled) {
+            compass.set_initial_location(gps.location().lat, gps.location().lng);
+        }
+        // set inertial nav home
+        inertial_nav.setup_home_position();
+        // update navigation scalers.  used to offset the shrinking longitude as we go towards the poles
+        scaleLongDown = longitude_scale(loc);
+        scaleLongUp   = 1.0f/scaleLongDown;
+        // record home is set
+        set_home_state(HOME_SET_NOT_LOCKED);
+    }
+
+    // To-Do: doing the stuff below constantly while armed could lead to lots of logging or performance hit?
 
     // log new home position which mission library will pull from ahrs
     if (should_log(MASK_LOG_CMD)) {
@@ -21,24 +106,50 @@ static void init_home()
         }
     }
 
-    // update navigation scalers.  used to offset the shrinking longitude as we go towards the poles
+    // return success
-    scaleLongDown = longitude_scale(loc);
+    return true;
-    scaleLongUp   = 1.0f/scaleLongDown;
 }
 
-// update_home - reset home to current location
+// far_from_EKF_origin - checks if a location is too far from the EKF origin
-//  should be called only when vehicle is disarmed
+//  returns true if too far
-static void update_home()
+static bool far_from_EKF_origin(const Location& loc)
 {
-    // copter uses 0 home altitude
+    // check distance to EKF origin
-    Location loc = gps.location();
+    const struct Location &ekf_origin = inertial_nav.get_origin();
-
+    if (get_distance(ekf_origin, loc) > EKF_ORIGIN_MAX_DIST_M) {
-    // set ahrs object's home position
+        return true;
-    ahrs.set_home(loc);
+    }
 
-    // update navigation scalers.  used to offset the shrinking longitude as we go towards the poles
+    // close enough to origin
-    scaleLongDown = longitude_scale(loc);
+    return false;
-    scaleLongUp   = 1.0f/scaleLongDown;
 }
 
+// checks if we should update ahrs/RTL home position from GPS
+static void set_system_time_from_GPS()
+{
+    // exit immediately if system time already set
+    if (ap.system_time_set) {
+        return;
+    }
+
+    // if we have a 3d lock and valid location
+    if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
+        // set system clock for log timestamps
+        hal.util->set_system_clock(gps.time_epoch_usec());
+        ap.system_time_set = true;
+        Log_Write_Event(DATA_SYSTEM_TIME_SET);
+    }
+}
 
+// check_gps_base_pos - sets gps base position (used for RTK only)
+static void check_gps_base_pos()
+{
+    if (!ap.gps_base_pos_set && !motors.armed() && home_is_set()) {
+        // if we're ready to enter RTK mode, then capture current state as home,
+        // and enter RTK fixes
+        if (gps.can_calculate_base_pos()) {
+            gps.calculate_base_pos();
+        }
+        ap.gps_base_pos_set = true;
+    }
+}

ArduCopter/config.h

@@ -334,6 +334,10 @@
  # define DCMCHECK_THRESHOLD_DEFAULT    0.8f    // DCM checker's default yaw error threshold above which we will abandon horizontal position hold.  The units are sin(angle) so 0.8 = about 60degrees of error
 #endif
 
+#ifndef EKF_ORIGIN_MAX_DIST_M
+ # define EKF_ORIGIN_MAX_DIST_M         50000   // EKF origin and waypoints (including home) must be within 50km
+#endif
+
 //////////////////////////////////////////////////////////////////////////////
 //  MAGNETOMETER
 #ifndef MAGNETOMETER

ArduCopter/defines.h

@@ -171,6 +171,13 @@
 #define WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL    2   // auto pilot will face next waypoint except when doing RTL at which time it will stay in it's last
 #define WP_YAW_BEHAVIOR_LOOK_AHEAD                    3   // auto pilot will look ahead during missions and rtl (primarily meant for traditional helicotpers)
 
+// home states (held in ap.home_state flags)
+enum HomeState {
+    HOME_UNSET,                 // home is unset, no GPS positions yet received
+    HOME_SET_NOT_LOCKED,        // home is set to EKF origin or armed location (can be moved)
+    HOME_SET_AND_LOCKED         // home has been set by user, cannot be moved except by user initiated do-set-home command
+};
+
 // Auto modes
 enum AutoMode {
     Auto_TakeOff,
@@ -261,6 +268,7 @@ enum FlipState {
 #define DATA_MAVLINK_INT16              3
 #define DATA_MAVLINK_INT8               4
 #define DATA_AP_STATE                   7
+#define DATA_SYSTEM_TIME_SET            8
 #define DATA_INIT_SIMPLE_BEARING        9
 #define DATA_ARMED                      10
 #define DATA_DISARMED                   11