Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: Reduce dependency on inertial nav

Browse Source
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 // get current location from EKF
Location temp_loc; Location temp_loc;
if (inertial_nav.get_location(temp_loc)) { Location ekf_origin;
const struct Location &ekf_origin = inertial_nav.get_origin(); if (ahrs.get_location(temp_loc) && ahrs.get_origin(ekf_origin)) {
temp_loc.alt = ekf_origin.alt; temp_loc.alt = ekf_origin.alt;
if (!set_home(temp_loc, false)) { if (!set_home(temp_loc, false)) {
// ignore this failure // ignore this failure
@ -38,7 +38,7 @@ bool Sub::set_home_to_current_location(bool lock)
{ {
// get current location from EKF // get current location from EKF
Location temp_loc; Location temp_loc;
if (inertial_nav.get_location(temp_loc)) { if (ahrs.get_location(temp_loc)) {
// Make home always at the water's surface. // Make home always at the water's surface.
// This allows disarming and arming again at depth. // 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) bool Sub::far_from_EKF_origin(const Location& loc)
{ {
// check distance to EKF origin // check distance to EKF origin
const struct Location &ekf_origin = inertial_nav.get_origin(); Location ekf_origin;
return (ekf_origin.get_distance(loc) > EKF_ORIGIN_MAX_DIST_M); 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() void Sub::read_inertia()
{ {
// inertial altitude estimates // inertial altitude estimates
inertial_nav.update(G_Dt); inertial_nav.update();
// pull position from interial nav library // pull position from ahrs
current_loc.lng = inertial_nav.get_longitude(); Location loc;
current_loc.lat = inertial_nav.get_latitude(); ahrs.get_position(loc);
current_loc.lat = loc.lat;
current_loc.lng = loc.lng;
// exit immediately if we do not have an altitude estimate // exit immediately if we do not have an altitude estimate
if (!inertial_nav.get_filter_status().flags.vert_pos) { 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 // pv_location_to_vector - convert lat/lon coordinates to a position vector
Vector3f Sub::pv_location_to_vector(const Location& loc) 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 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); 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 // pv_alt_above_origin - convert altitude above home to altitude above EKF origin
float Sub::pv_alt_above_origin(float alt_above_home_cm) 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); return alt_above_home_cm + (ahrs.get_home().alt - origin.alt);
} }