mirror of https://github.com/ArduPilot/ardupilot
Plane: prevent a discrepancy between EKF origin and home for altitude
This commit is contained in:
parent
ba3576f027
commit
f96836ab9a
|
@ -131,7 +131,15 @@ void Plane::init_home()
|
||||||
void Plane::update_home()
|
void Plane::update_home()
|
||||||
{
|
{
|
||||||
if (home_is_set == HOME_SET_NOT_LOCKED) {
|
if (home_is_set == HOME_SET_NOT_LOCKED) {
|
||||||
ahrs.set_home(gps.location());
|
Location loc = gps.location();
|
||||||
|
Location origin;
|
||||||
|
// if an EKF origin is available then we leave home equal to
|
||||||
|
// the height of that origin. This ensures that our relative
|
||||||
|
// height calculations are using the same origin
|
||||||
|
if (ahrs.get_origin(origin)) {
|
||||||
|
loc.alt = origin.alt;
|
||||||
|
}
|
||||||
|
ahrs.set_home(loc);
|
||||||
Log_Write_Home_And_Origin();
|
Log_Write_Home_And_Origin();
|
||||||
GCS_MAVLINK::send_home_all(gps.location());
|
GCS_MAVLINK::send_home_all(gps.location());
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue