Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 09:58:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: set ahrs home from ekf location

Browse Source
This commit is contained in:
Randy Mackay 2015-02-25 23:13:59 +09:00
parent e5ddd276fd
commit 2bec00e1c5
2 changed files with 12 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/ArduCopter.pde
View File

@ -1177,7 +1177,7 @@ static void update_GPS(void)
set_system_time_from_GPS(); set_system_time_from_GPS();
// update home from GPS location if necessary // update home from GPS location if necessary
update_home_from_GPS(); update_home_from_EKF();
// check gps base position (used for RTK only) // check gps base position (used for RTK only)
check_gps_base_pos(); check_gps_base_pos();

40
ArduCopter/commands.pde
View File

@ -7,44 +7,26 @@
* HOME_SET_AND_LOCKED = home has been set by user, cannot be moved except by user initiated do-set-home command * 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 the EKF
static void update_home_from_EKF()
// 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 // exit immediately if home already set
if (ground_start_count == 0 && ap.home_state != HOME_UNSET) { if (ap.home_state != HOME_UNSET) {
return; return;
} }
// if counter has not run down // move home to current ekf location (this will set home_state to HOME_SET)
if (ground_start_count > 0) { set_home_to_current_location();
// 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 // set_home_to_current_location - set home to current GPS location
static bool set_home_to_current_location() { static bool set_home_to_current_location() {
// exit with failure if we haven't had 10 good GPS position // get current location from EKF
if (ground_start_count > 0) { Location temp_loc;
return false; if (inertial_nav.get_location(temp_loc)) {
return set_home(temp_loc);
} }
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 // set_home_to_current_location_and_lock - set home to current location and lock so it cannot be moved