Rover: set home from ekf position

APMrover2/APMrover2.cpp

@@ -173,6 +173,9 @@ void Rover::ahrs_update()
 
     ahrs.update();
 
+    // update home from EKF if necessary
+    update_home_from_EKF();
+
     // if using the EKF get a speed update now (from accelerometers)
     Vector3f velocity;
     if (ahrs.get_velocity_NED(velocity)) {
@@ -399,44 +402,23 @@ void Rover::update_GPS_10Hz(void)
 {
     have_position = ahrs.get_position(current_loc);
 
-    if (gps.last_message_time_ms() != last_gps_msg_ms && gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
+    if (gps.last_message_time_ms() != last_gps_msg_ms) {
         last_gps_msg_ms = gps.last_message_time_ms();
 
-        if (ground_start_count > 1) {
-            ground_start_count--;
-
-        } else if (ground_start_count == 1) {
-            // We countdown N number of good GPS fixes
-            // so that the altitude is more accurate
-            // -------------------------------------
-            if (current_loc.lat == 0 && current_loc.lng == 0) {
-                ground_start_count = 20;
-            } else {
-                init_home();
-
-                // set system clock for log timestamps
-                const uint64_t gps_timestamp = gps.time_epoch_usec();
+        // set system time if necessary
+        set_system_time_from_GPS();
 
-                hal.util->set_system_clock(gps_timestamp);
+        if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
 
-                // update signing timestamp
-                GCS_MAVLINK::update_signing_timestamp(gps_timestamp);
-
-                if (g.compass_enabled) {
-                    // Set compass declination automatically
-                    compass.set_initial_location(gps.location().lat, gps.location().lng);
-                }
-                ground_start_count = 0;
-            }
-        }
-        // get ground speed estimate from AHRS
-        ground_speed = ahrs.groundspeed();
+            // get ground speed estimate from AHRS
+            ground_speed = ahrs.groundspeed();
 
 #if CAMERA == ENABLED
-        if (camera.update_location(current_loc, rover.ahrs) == true) {
-            do_take_picture();
-        }
+            if (camera.update_location(current_loc, rover.ahrs) == true) {
+                do_take_picture();
+            }
 #endif
+        }
     }
 }
 

APMrover2/GCS_Mavlink.cpp

@@ -1046,29 +1046,17 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
             // param7 : altitude
             result = MAV_RESULT_FAILED;  // assume failure
             if (is_equal(packet.param1, 1.0f)) {
-                rover.init_home();
-            } else {
-                if (is_zero(packet.param5) && is_zero(packet.param6) && is_zero(packet.param7)) {
-                    // don't allow the 0,0 position
-                    break;
-                }
-                // sanity check location
-                if (!check_latlng(packet.param5, packet.param6)) {
-                    break;
+                if (rover.set_home_to_current_location(true)) {
+                    result = MAV_RESULT_ACCEPTED;
                 }
+            } else {
                 Location new_home_loc {};
                 new_home_loc.lat = static_cast<int32_t>(packet.param5 * 1.0e7f);
                 new_home_loc.lng = static_cast<int32_t>(packet.param6 * 1.0e7f);
                 new_home_loc.alt = static_cast<int32_t>(packet.param7 * 100.0f);
-                rover.ahrs.set_home(new_home_loc);
-                rover.home_is_set = HOME_SET_NOT_LOCKED;
-                rover.Log_Write_Home_And_Origin();
-                GCS_MAVLINK::send_home_all(new_home_loc);
-                result = MAV_RESULT_ACCEPTED;
-                rover.gcs_send_text_fmt(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %.2fm",
-                        static_cast<double>(new_home_loc.lat * 1.0e-7f),
-                        static_cast<double>(new_home_loc.lng * 1.0e-7f),
-                        static_cast<double>(new_home_loc.alt * 0.01f));
+                if (rover.set_home(new_home_loc, true)) {
+                    result = MAV_RESULT_ACCEPTED;
+                }
             }
             break;
         }

APMrover2/Rover.cpp

@@ -43,7 +43,6 @@ Rover::Rover(void) :
     camera_mount(ahrs, current_loc),
 #endif
     control_mode(INITIALISING),
-    ground_start_count(20),
     throttle(500),
 #if FRSKY_TELEM_ENABLED == ENABLED
     frsky_telemetry(ahrs, battery, sonar),

APMrover2/Rover.h

@@ -243,10 +243,6 @@ private:
     // notification object for LEDs, buzzers etc (parameter set to false disables external leds)
     AP_Notify notify;
 
-    // A counter used to count down valid gps fixes to allow the gps estimate to settle
-    // before recording our home position (and executing a ground start if we booted with an air start)
-    uint8_t ground_start_count;
-
     // true if we have a position estimate from AHRS
     bool have_position;
 
@@ -335,6 +331,9 @@ private:
     // Flag for if we have g_gps lock and have set the home location in AHRS
     enum HomeState home_is_set = HOME_UNSET;
 
+    // true if the system time has been set from the GPS
+    bool system_time_set;
+
     // The location of the previous waypoint.  Used for track following and altitude ramp calculations
     struct Location prev_WP;
     // The location of the current/active waypoint.  Used for track following
@@ -476,7 +475,10 @@ private:
     void set_auto_WP(const struct Location& loc);
     void set_guided_WP(const struct Location& loc);
     void set_guided_velocity(float target_steer_speed, float target_speed);
-    void init_home();
+    void update_home_from_EKF();
+    bool set_home_to_current_location(bool lock);
+    bool set_home(const Location& loc, bool lock);
+    void set_system_time_from_GPS();
     void restart_nav();
     void exit_mission();
     void do_RTL(void);

APMrover2/commands.cpp

@@ -60,32 +60,118 @@ void Rover::set_guided_velocity(float target_steer_speed, float target_speed)
     rover.rtl_complete = false;
 }
 
-// run this at setup on the ground
-// -------------------------------
-void Rover::init_home()
+// checks if we should update ahrs home position from the EKF's position
+void Rover::update_home_from_EKF()
 {
-    if (!have_position) {
-        // we need position information
+    // exit immediately if home already set
+    if (home_is_set != HOME_UNSET) {
         return;
     }
 
-    gcs_send_text(MAV_SEVERITY_INFO, "Init HOME");
+    // move home to current ekf location (this will set home_state to HOME_SET)
+    set_home_to_current_location(false);
+}
 
-    ahrs.set_home(gps.location());
-    home_is_set = HOME_SET_NOT_LOCKED;
-    Log_Write_Home_And_Origin();
-    GCS_MAVLINK::send_home_all(gps.location());
+// set ahrs home to current location from EKF reported location or GPS
+bool Rover::set_home_to_current_location(bool lock)
+{
+    // use position from EKF if available otherwise use GPS
+    Location temp_loc;
+    if (ahrs.get_position(temp_loc)) {
+        return set_home(temp_loc, lock);
+    }
+    return false;
+}
+
+// sets ahrs home to specified location
+//  returns true if home location set successfully
+bool Rover::set_home(const Location& loc, bool lock)
+{
+    // check location is valid
+    if (loc.lat == 0 && loc.lng == 0 && loc.alt == 0) {
+        return false;
+    }
+    if (!check_latlng(loc)) {
+        return false;
+    }
+
+    // set EKF origin to home if it hasn't been set yet
+    Location ekf_origin;
+    if (!ahrs.get_origin(ekf_origin)) {
+        ahrs.set_origin(loc);
+    }
+
+    // set ahrs home
+    ahrs.set_home(loc);
+
+    // init compass declination
+    if (home_is_set == HOME_UNSET) {
+        // Set compass declination automatically
+        if (g.compass_enabled) {
+            compass.set_initial_location(loc.lat, loc.lng);
+        }
+        // record home is set
+        home_is_set = HOME_SET_NOT_LOCKED;
+
+        // log new home position which mission library will pull from ahrs
+        if (should_log(MASK_LOG_CMD)) {
+            AP_Mission::Mission_Command temp_cmd;
+            if (mission.read_cmd_from_storage(0, temp_cmd)) {
+                DataFlash.Log_Write_Mission_Cmd(mission, temp_cmd);
+            }
+        }
+
+        // initialise navigation to home
+        next_WP = prev_WP = home;
+
+        // Load home for a default guided_WP
+        set_guided_WP(home);
+    }
+
+    // lock home position
+    if (lock) {
+        home_is_set = HOME_SET_AND_LOCKED;
+    }
 
     // Save Home to EEPROM
     mission.write_home_to_storage();
 
-    // Save prev loc
-    // -------------
-    next_WP = prev_WP = home;
+    // log ahrs home and ekf origin dataflash
+    Log_Write_Home_And_Origin();
+
+    // send new home location to GCS
+    GCS_MAVLINK::send_home_all(loc);
+
+    // send text of home position to ground stations
+    rover.gcs_send_text_fmt(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %.2fm",
+            static_cast<double>(loc.lat * 1.0e-7f),
+            static_cast<double>(loc.lng * 1.0e-7f),
+            static_cast<double>(loc.alt * 0.01f));
+
+    // return success
+    return true;
+}
 
-    // Load home for a default guided_WP
-    // -------------
-    set_guided_WP(home);
+// checks if we should update ahrs/RTL home position from GPS
+void Rover::set_system_time_from_GPS()
+{
+    // exit immediately if system time already set
+    if (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
+        const uint64_t gps_timestamp = gps.time_epoch_usec();
+
+        hal.util->set_system_clock(gps_timestamp);
+
+        // update signing timestamp
+        GCS_MAVLINK::update_signing_timestamp(gps_timestamp);
+
+        system_time_set = true;
+    }
 }
 
 void Rover::restart_nav()

APMrover2/commands_logic.cpp

@@ -542,12 +542,9 @@ void Rover::do_change_speed(const AP_Mission::Mission_Command& cmd)
 void Rover::do_set_home(const AP_Mission::Mission_Command& cmd)
 {
     if (cmd.p1 == 1 && have_position) {
-        init_home();
+        set_home_to_current_location(false);
     } else {
-        ahrs.set_home(cmd.content.location);
-        home_is_set = HOME_SET_NOT_LOCKED;
-        Log_Write_Home_And_Origin();
-        GCS_MAVLINK::send_home_all(cmd.content.location);
+        set_home(cmd.content.location, false);
     }
 }
 

APMrover2/control_modes.cpp

@@ -99,7 +99,7 @@ void Rover::read_trim_switch()
                     mission.clear();
                     if (channel_steer->get_control_in() > 3000) {
                         // if roll is full right store the current location as home
-                        init_home();
+                        set_home_to_current_location(false);
                     }
                     break;