diff --git a/Blimp/inertia.cpp b/Blimp/inertia.cpp index b46e03978d..05fa634384 100644 --- a/Blimp/inertia.cpp +++ b/Blimp/inertia.cpp @@ -20,7 +20,7 @@ void Blimp::read_inertia() // 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_position_z_up_cm(); current_loc.set_alt_cm(alt_above_origin_cm, Location::AltFrame::ABOVE_ORIGIN); - if (!ahrs.home_is_set() || !current_loc.change_alt_frame(Location::AltFrame::ABOVE_HOME)) { + if (current_loc.initialised() && !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); }