Copter: correct current_loc to be alt-above-home

This commit is contained in:
Randy Mackay 2020-02-01 16:55:28 +09:00
parent 103fa8b3d9
commit bd8bcd5ab1
1 changed files with 6 additions and 7 deletions

View File

@ -17,12 +17,11 @@ void Copter::read_inertia()
return;
}
if (ahrs.home_is_set()) {
current_loc.set_alt_cm(inertial_nav.get_altitude(),
Location::AltFrame::ABOVE_HOME);
} else {
// without home use alt above the EKF origin
current_loc.set_alt_cm(inertial_nav.get_altitude(),
Location::AltFrame::ABOVE_ORIGIN);
// current_loc.alt is alt-above-home, converted from inertial nav's alt-above-ekf-origin
const int32_t alt_above_origin_cm = inertial_nav.get_altitude();
current_loc.set_alt_cm(alt_above_origin_cm, Location::AltFrame::ABOVE_ORIGIN);
if (!current_loc.change_alt_frame(Location::AltFrame::ABOVE_HOME)) {
// if home has not been set yet we treat alt-above-origin as alt-above-home
current_loc.set_alt_cm(alt_above_origin_cm, Location::AltFrame::ABOVE_HOME);
}
}