Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 12:14:10 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: remove position-vector methods

Browse Source 
Both were used in just one place
This commit is contained in:
Peter Barker 2019-02-21 17:42:13 +11:00 committed by Peter Barker
parent 6fc4086726
commit a48f461158
4 changed files with 13 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/Copter.h
View File

@ -788,10 +788,6 @@ private:
void convert_pid_parameters(void);
void convert_lgr_parameters(void);
// position_vector.cpp
float pv_alt_above_origin(float alt_above_home_cm);
float pv_alt_above_home(float alt_above_origin_cm);
// precision_landing.cpp
void init_precland();
void update_precland();

7
ArduCopter/GCS_Mavlink.cpp
View File

@ -1033,7 +1033,12 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
pos_vector += copter.inertial_nav.get_position();
} else {
// convert from alt-above-home to alt-above-ekf-origin
pos_vector.z = copter.pv_alt_above_origin(pos_vector.z);
if (!AP::ahrs().home_is_set()) {
break;
}
const Location &origin = copter.inertial_nav.get_origin();
pos_vector.z += AP::ahrs().get_home().alt;
pos_vector.z -= origin.alt;
}
}

14
ArduCopter/inertia.cpp
View File

@ -15,16 +15,16 @@ void Copter::read_inertia()
return;
}
// without home return alt above the EKF origin
if (!ahrs.home_is_set()) {
// with inertial nav we can update the altitude and climb rate at 50hz
current_loc.alt = inertial_nav.get_altitude();
Location::ALT_FRAME frame;
if (ahrs.home_is_set()) {
frame = Location::ALT_FRAME_ABOVE_HOME;
} 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());
// without home use alt above the EKF origin
frame = Location::ALT_FRAME_ABOVE_ORIGIN;
}
current_loc.set_alt_cm(inertial_nav.get_altitude(), frame);
current_loc.change_alt_frame(Location::ALT_FRAME_ABOVE_HOME);
// set flags and get velocity
current_loc.relative_alt = true;
climb_rate = inertial_nav.get_velocity_z();
}

23
ArduCopter/position_vector.cpp
View File

@ -1,23 +0,0 @@
#include "Copter.h"
// Position vectors related utility functions
// position vectors are Vector3f
// .x = latitude from home in cm
// .y = longitude from home in cm
// .z = altitude above home in cm
// pv_alt_above_origin - convert altitude above home to altitude above EKF origin
float Copter::pv_alt_above_origin(float alt_above_home_cm)
{
const struct Location &origin = inertial_nav.get_origin();
return alt_above_home_cm + (ahrs.get_home().alt - origin.alt);
}
// pv_alt_above_home - convert altitude above EKF origin to altitude above home
float Copter::pv_alt_above_home(float alt_above_origin_cm)
{
const struct Location &origin = inertial_nav.get_origin();
return alt_above_origin_cm + (origin.alt - ahrs.get_home().alt);
}