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

6
ArduCopter/GCS_Mavlink.cpp
View File

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

10
ArduCopter/commands.cpp
View File

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

10
ArduCopter/inertia.cpp
View File

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