mirror of https://github.com/ArduPilot/ardupilot
Sub: home altitude is always 0, referenced from water's surface
This commit is contained in:
parent
ad60e8476f
commit
3da4dc7089
|
@ -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;
|
||||
|
|
|
@ -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();
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue