Rover: attempt to set home position regardless of GPS lock

The AHRS may be supplying as a home position through mechanisms other
than a GPS lock.  Don't assume in the caller to update_home().

Rover: fold update_home_from_EKF back into sole caller

This method is confusingly similarly named to update_home - and they do
pretty much the opposite thing.

Rover: remove incorrect and misleading comments on set_home_to_current_location

Rover: rewrite update_home to not set home if no origin set

Also checks that home is set before using values from it

Rover: zero stack variables when updating home

If these are a problem we have significant problems through the code
This commit is contained in:
Peter Barker 2018-11-08 09:41:48 +11:00 committed by Peter Barker
parent 44b7763caf
commit 6fcdfbbe32
4 changed files with 27 additions and 40 deletions

View File

@ -164,8 +164,10 @@ void Rover::ahrs_update()
// update position // update position
have_position = ahrs.get_position(current_loc); have_position = ahrs.get_position(current_loc);
// update home from EKF if necessary // set home from EKF if necessary and possible
update_home_from_EKF(); if (!ahrs.home_is_set()) {
set_home_to_current_location(false);
}
// if using the EKF get a speed update now (from accelerometers) // if using the EKF get a speed update now (from accelerometers)
Vector3f velocity; Vector3f velocity;
@ -281,10 +283,8 @@ void Rover::one_second_loop(void)
// cope with changes to mavlink system ID // cope with changes to mavlink system ID
mavlink_system.sysid = g.sysid_this_mav; mavlink_system.sysid = g.sysid_this_mav;
// update home position if not soft armed and gps position has // attempt to update home position and baro calibration if not armed:
// changed. Update every 1s at most if (!hal.util->get_soft_armed()) {
if (!hal.util->get_soft_armed() &&
gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
update_home(); update_home();
} }

View File

@ -406,7 +406,6 @@ private:
void update_mission(void); void update_mission(void);
// commands.cpp // commands.cpp
void update_home_from_EKF();
bool set_home_to_current_location(bool lock); bool set_home_to_current_location(bool lock);
bool set_home(const Location& loc, bool lock); bool set_home(const Location& loc, bool lock);
void update_home(); void update_home();

View File

@ -1,21 +1,8 @@
#include "Rover.h" #include "Rover.h"
// checks if we should update ahrs home position from the EKF's position // set ahrs home to current location from inertial-nav location
void Rover::update_home_from_EKF()
{
// exit immediately if home already set
if (ahrs.home_is_set()) {
return;
}
// move home to current ekf location (this will set home_state to HOME_SET)
set_home_to_current_location(false);
}
// set ahrs home to current location from EKF reported location or GPS
bool Rover::set_home_to_current_location(bool lock) bool Rover::set_home_to_current_location(bool lock)
{ {
// use position from EKF if available otherwise use GPS
Location temp_loc; Location temp_loc;
if (ahrs.have_inertial_nav() && ahrs.get_position(temp_loc)) { if (ahrs.have_inertial_nav() && ahrs.get_position(temp_loc)) {
if (!set_home(temp_loc, lock)) { if (!set_home(temp_loc, lock)) {
@ -40,12 +27,6 @@ bool Rover::set_home(const Location& loc, bool lock)
return false; return false;
} }
// check if EKF origin has been set
Location ekf_origin;
if (!ahrs.get_origin(ekf_origin)) {
return false;
}
const bool home_was_set = ahrs.home_is_set(); const bool home_was_set = ahrs.home_is_set();
// set ahrs home // set ahrs home
@ -79,20 +60,27 @@ bool Rover::set_home(const Location& loc, bool lock)
return true; return true;
} }
/* // called periodically while disarmed to update our home position to
update home location from GPS // our current location
this is called as long as we have 3D lock and the arming switch is
not pushed
*/
void Rover::update_home() void Rover::update_home()
{ {
if (!ahrs.home_is_locked()) { if (ahrs.home_is_locked()) {
Location loc; // we've been explicitly told our home location
if (ahrs.get_position(loc)) { return;
if (get_distance(loc, ahrs.get_home()) > DISTANCE_HOME_MAX) {
ahrs.set_home(loc);
}
} }
Location loc{};
if (!ahrs.get_position(loc)) {
return;
} }
barometer.update_calibration(); barometer.update_calibration();
if (ahrs.home_is_set() &&
get_distance(loc, ahrs.get_home()) < DISTANCE_HOME_MINCHANGE) {
// insufficiently moved from current home - don't change it
return;
}
ahrs.set_home(loc);
} }

View File

@ -109,7 +109,7 @@ enum fs_ekf_action {
FS_EFK_HOLD = 1 FS_EFK_HOLD = 1
}; };
#define DISTANCE_HOME_MAX 0.5f // Distance max to home location before changing it when disarm #define DISTANCE_HOME_MINCHANGE 0.5f // minimum distance to adjust home location
enum mode_reason_t { enum mode_reason_t {
MODE_REASON_INITIALISED = 0, MODE_REASON_INITIALISED = 0,