Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: set ahrs home from ekf location

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();
// 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_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
*/
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()
// checks if we should update ahrs/RTL home position from the EKF
static void update_home_from_EKF()
{
// exit immediately if counter has run down and home already set
if (ground_start_count == 0 && ap.home_state != HOME_UNSET) {
// exit immediately if home already set
if (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());
// move home to current ekf location (this will set home_state to HOME_SET)
set_home_to_current_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;
// get current location from EKF
Location temp_loc;
if (inertial_nav.get_location(temp_loc)) {
return set_home(temp_loc);
}
// set home to latest gps location
return set_home(gps.location());
return false;
}
// set_home_to_current_location_and_lock - set home to current location and lock so it cannot be moved