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

Sub: home altitude is always 0, referenced from water's surface

This commit is contained in:
Jacob Walser 2016-10-10 23:27:51 -04:00 committed by Andrew Tridgell
parent ad60e8476f
commit 3da4dc7089
2 changed files with 9 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduSub/commands.cpp
View File

@ -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;

13
ArduSub/inertia.cpp
View File

@ -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();
}