Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: remove pv_ functions that duplicate Location functions

Browse Source
This commit is contained in:
Peter Barker 2019-02-05 09:08:19 +11:00 committed by Randy Mackay
parent 66778dbf63
commit 587e02e92e
5 changed files with 14 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/Copter.h
View File

@ -807,10 +807,8 @@ private:
void convert_lgr_parameters(void);
// position_vector.cpp
Vector3f pv_location_to_vector(const Location& loc);
float pv_alt_above_origin(float alt_above_home_cm);
float pv_alt_above_home(float alt_above_origin_cm);
float pv_distance_to_home_cm(const Vector3f &destination);
// precision_landing.cpp
void init_precland();

9
ArduCopter/GCS_Mavlink.cpp
View File

@ -1197,14 +1197,11 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
case MAV_FRAME_GLOBAL:
case MAV_FRAME_GLOBAL_INT:
default:
// pv_location_to_vector does not support absolute altitudes.
// Convert the absolute altitude to a home-relative altitude before calling pv_location_to_vector
loc.alt -= copter.ahrs.get_home().alt;
loc.relative_alt = true;
loc.terrain_alt = false;
break;
}
pos_neu_cm = copter.pv_location_to_vector(loc);
if (!loc.get_vector_from_origin_NEU(pos_neu_cm)) {
break;
}
}
// prepare yaw

10
ArduCopter/autoyaw.cpp
View File

@ -146,8 +146,9 @@ void Copter::Mode::AutoYaw::set_roi(const Location &roi_location)
#if MOUNT == ENABLED
// check if mount type requires us to rotate the quad
if (!copter.camera_mount.has_pan_control()) {
roi = copter.pv_location_to_vector(roi_location);
auto_yaw.set_mode(AUTO_YAW_ROI);
if (roi_location.get_vector_from_origin_NEU(roi)) {
auto_yaw.set_mode(AUTO_YAW_ROI);
}
}
// send the command to the camera mount
copter.camera_mount.set_roi_target(roi_location);
@ -160,8 +161,9 @@ void Copter::Mode::AutoYaw::set_roi(const Location &roi_location)
// 4: point at a target given a target id (can't be implemented)
#else
// if we have no camera mount aim the quad at the location
roi = copter.pv_location_to_vector(roi_location);
auto_yaw.set_mode(AUTO_YAW_ROI);
if (roi_location.get_vector_from_origin_NEU(roi)) {
auto_yaw.set_mode(AUTO_YAW_ROI);
}
#endif // MOUNT == ENABLED
}
}

7
ArduCopter/mode_auto.cpp
View File

@ -1787,9 +1787,12 @@ bool Copter::ModeAuto::verify_circle(const AP_Mission::Mission_Command& cmd)
// check if we've reached the edge
if (mode() == Auto_CircleMoveToEdge) {
if (copter.wp_nav->reached_wp_destination()) {
Vector3f circle_center;
if (!cmd.content.location.get_vector_from_origin_NEU(circle_center)) {
// should never happen
return true;
}
const Vector3f curr_pos = copter.inertial_nav.get_position();
Vector3f circle_center = copter.pv_location_to_vector(cmd.content.location);
// set target altitude if not provided
if (is_zero(circle_center.z)) {
circle_center.z = curr_pos.z;

14
ArduCopter/position_vector.cpp
View File

@ -7,13 +7,6 @@
// .y = longitude from home in cm
// .z = altitude above home in cm
// pv_location_to_vector - convert lat/lon coordinates to a position vector
Vector3f Copter::pv_location_to_vector(const Location& loc)
{
const struct Location &origin = inertial_nav.get_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);
}
// pv_alt_above_origin - convert altitude above home to altitude above EKF origin
float Copter::pv_alt_above_origin(float alt_above_home_cm)
@ -28,10 +21,3 @@ 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);
}
// returns distance between a destination and home in cm
float Copter::pv_distance_to_home_cm(const Vector3f &destination)
{
Vector3f home = pv_location_to_vector(ahrs.get_home());
return get_horizontal_distance_cm(home, destination);
}