aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: Reduce dependency on inertial nav

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

4
ArduCopter/GCS_Mavlink.cpp
View File

@ -993,11 +993,13 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
if (!AP::ahrs().home_is_set()) { if (!AP::ahrs().home_is_set()) {
break; break;
} }
const Location &origin = copter.inertial_nav.get_origin(); Location origin;
pos_vector.z += AP::ahrs().get_home().alt; pos_vector.z += AP::ahrs().get_home().alt;
if (copter.ahrs.get_origin(origin)) {
pos_vector.z -= origin.alt; pos_vector.z -= origin.alt;
} }
} }
}
// prepare velocity // prepare velocity
Vector3f vel_vector; Vector3f vel_vector;

10
ArduCopter/commands.cpp
View File

@ -23,8 +23,8 @@ void Copter::update_home_from_EKF()
void Copter::set_home_to_current_location_inflight() { void Copter::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)) {
return; return;
@ -40,7 +40,7 @@ void Copter::set_home_to_current_location_inflight() {
bool Copter::set_home_to_current_location(bool lock) { bool Copter::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)) {
if (!set_home(temp_loc, lock)) { if (!set_home(temp_loc, lock)) {
return false; return false;
} }
@ -106,8 +106,8 @@ bool Copter::set_home(const Location& loc, bool lock)
bool Copter::far_from_EKF_origin(const Location& loc) bool Copter::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;
if (ekf_origin.get_distance(loc) > EKF_ORIGIN_MAX_DIST_M) { if (ahrs.get_origin(ekf_origin) && (ekf_origin.get_distance(loc) > EKF_ORIGIN_MAX_DIST_M)) {
return true; return true;
} }

10
ArduCopter/inertia.cpp
View File

@ -4,11 +4,13 @@
void Copter::read_inertia() void Copter::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) {