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

Sub: Reduce dependency on inertial nav

This commit is contained in:
Michael du Breuil 2019-06-28 13:55:49 +00:00 committed by Randy Mackay
parent e0d7b71e95
commit bbcb976e73
3 changed files with 19 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduSub/commands.cpp
View File

@ -24,8 +24,8 @@ void Sub::set_home_to_current_location_inflight()
{
// get current location from EKF
Location temp_loc;
if (inertial_nav.get_location(temp_loc)) {
const struct Location &ekf_origin = inertial_nav.get_origin();
Location ekf_origin;
if (ahrs.get_location(temp_loc) && ahrs.get_origin(ekf_origin)) {
temp_loc.alt = ekf_origin.alt;
if (!set_home(temp_loc, false)) {
// ignore this failure
@ -38,7 +38,7 @@ bool Sub::set_home_to_current_location(bool lock)
{
// get current location from EKF
Location temp_loc;
if (inertial_nav.get_location(temp_loc)) {
if (ahrs.get_location(temp_loc)) {
// Make home always at the water's surface.
// This allows disarming and arming again at depth.
@ -98,6 +98,6 @@ bool Sub::set_home(const Location& loc, bool lock)
bool Sub::far_from_EKF_origin(const Location& loc)
{
// check distance to EKF origin
const struct Location &ekf_origin = inertial_nav.get_origin();
return (ekf_origin.get_distance(loc) > EKF_ORIGIN_MAX_DIST_M);
Location ekf_origin;
return ahrs.get_origin(ekf_origin) && (ekf_origin.get_distance(loc) > EKF_ORIGIN_MAX_DIST_M);
}

10
ArduSub/inertia.cpp
View File

@ -4,11 +4,13 @@
void Sub::read_inertia()
{
// inertial altitude estimates
inertial_nav.update(G_Dt);
inertial_nav.update();
// pull position from interial nav library
current_loc.lng = inertial_nav.get_longitude();
current_loc.lat = inertial_nav.get_latitude();
// pull position from ahrs
Location loc;
ahrs.get_position(loc);
current_loc.lat = loc.lat;
current_loc.lng = loc.lng;
// exit immediately if we do not have an altitude estimate
if (!inertial_nav.get_filter_status().flags.vert_pos) {

10
ArduSub/position_vector.cpp
View File

@ -10,7 +10,10 @@
// pv_location_to_vector - convert lat/lon coordinates to a position vector
Vector3f Sub::pv_location_to_vector(const Location& loc)
{
const struct Location &origin = inertial_nav.get_origin();
Location origin;
if (!ahrs.get_origin(origin)) {
origin.zero();
}
float alt_above_origin = pv_alt_above_origin(loc.alt); // convert alt-relative-to-home to alt-relative-to-origin
return Vector3f((loc.lat-origin.lat) * LATLON_TO_CM, (loc.lng-origin.lng) * LATLON_TO_CM * scaleLongDown, alt_above_origin);
}
@ -18,7 +21,10 @@ Vector3f Sub::pv_location_to_vector(const Location& loc)
// pv_alt_above_origin - convert altitude above home to altitude above EKF origin
float Sub::pv_alt_above_origin(float alt_above_home_cm)
{
const struct Location &origin = inertial_nav.get_origin();
Location origin;
if (!ahrs.get_origin(origin)) {
origin.zero();
}
return alt_above_home_cm + (ahrs.get_home().alt - origin.alt);
}