From 3da4dc708968e5987d245918f92ddd4f00e13c24 Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Mon, 10 Oct 2016 23:27:51 -0400 Subject: [PATCH] Sub: home altitude is always 0, referenced from water's surface --- ArduSub/commands.cpp | 6 ++++++ ArduSub/inertia.cpp | 13 +++---------- 2 files changed, 9 insertions(+), 10 deletions(-) diff --git a/ArduSub/commands.cpp b/ArduSub/commands.cpp index 8e3bcc50a3..6e4694d1ce 100644 --- a/ArduSub/commands.cpp +++ b/ArduSub/commands.cpp @@ -42,6 +42,12 @@ bool Sub::set_home_to_current_location() { // get current location from EKF Location temp_loc; if (inertial_nav.get_location(temp_loc)) { + + // Make home always at the water's surface. + // This allows disarming and arming again at depth. + // This also ensures that mission items with relative altitude frame, are always + // relative to the water's surface, whether in a high elevation lake, or at sea level. + temp_loc.alt -= barometer.get_altitude() * 100.0f; return set_home(temp_loc); } return false; diff --git a/ArduSub/inertia.cpp b/ArduSub/inertia.cpp index 5853ff0f33..cb4301219c 100644 --- a/ArduSub/inertia.cpp +++ b/ArduSub/inertia.cpp @@ -17,16 +17,9 @@ void Sub::read_inertia() return; } - // without home return alt above the EKF origin - if (ap.home_state == HOME_UNSET) { - // with inertial nav we can update the altitude and climb rate at 50hz - current_loc.alt = inertial_nav.get_altitude(); - } else { - // with inertial nav we can update the altitude and climb rate at 50hz - current_loc.alt = pv_alt_above_home(inertial_nav.get_altitude()); - } + current_loc.alt = inertial_nav.get_altitude(); - // set flags and get velocity - current_loc.flags.relative_alt = true; + // get velocity, altitude is always absolute frame, referenced from + // water's surface climb_rate = inertial_nav.get_velocity_z(); }