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: remove position_vector.cpp

Browse Source
This commit is contained in:
Joshua Henderson 2022-02-05 00:35:32 -05:00 committed by Andrew Tridgell
parent 2836ec4ae0
commit fc5ecfcafd
4 changed files with 10 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduSub/Sub.h
View File

@ -516,8 +516,6 @@ private:
void set_surfaced(bool at_surface);
void set_bottomed(bool at_bottom);
void motors_output();
Vector3f pv_location_to_vector(const Location& loc);
float pv_alt_above_origin(float alt_above_home_cm);
void init_rc_in();
void init_rc_out();
void enable_motor_output();

3
ArduSub/commands_logic.cpp
View File

@ -509,7 +509,8 @@ bool Sub::verify_circle(const AP_Mission::Mission_Command& cmd)
// check if we've reached the edge
if (auto_mode == Auto_CircleMoveToEdge) {
if (wp_nav.reached_wp_destination()) {
Vector3f circle_center = pv_location_to_vector(cmd.content.location);
Vector3f circle_center;
UNUSED_RESULT(cmd.content.location.get_vector_from_origin_NEU(circle_center));
// set target altitude if not provided
if (is_zero(circle_center.z)) {

14
ArduSub/control_auto.cpp
View File

@ -463,10 +463,11 @@ void Sub::set_auto_yaw_roi(const Location &roi_location)
#endif // HAL_MOUNT_ENABLED
} else {
#if HAL_MOUNT_ENABLED
// check if mount type requires us to rotate the quad
// check if mount type requires us to rotate the sub
if (!camera_mount.has_pan_control()) {
roi_WP = pv_location_to_vector(roi_location);
set_auto_yaw_mode(AUTO_YAW_ROI);
if (roi_location.get_vector_from_origin_NEU(roi_WP)) {
set_auto_yaw_mode(AUTO_YAW_ROI);
}
}
// send the command to the camera mount
camera_mount.set_roi_target(roi_location);
@ -478,9 +479,10 @@ void Sub::set_auto_yaw_roi(const Location &roi_location)
// 3: point at a location given by alt, lon, lat parameters
// 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_WP = pv_location_to_vector(roi_location);
set_auto_yaw_mode(AUTO_YAW_ROI);
// if we have no camera mount aim the sub at the location
if (roi_location.get_vector_from_origin_NEU(roi_WP)) {
set_auto_yaw_mode(AUTO_YAW_ROI);
}
#endif // HAL_MOUNT_ENABLED
}
}

33
ArduSub/position_vector.cpp
View File

@ -1,33 +0,0 @@
#include "Sub.h"
// position_vector.pde 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_location_to_vector - convert lat/lon coordinates to a position vector
Vector3f Sub::pv_location_to_vector(const Location& loc)
{
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
Vector3f vec = origin.get_distance_NED(loc);
vec.xy() *= 100;
vec.z = alt_above_origin;
return vec;
}
// pv_alt_above_origin - convert altitude above home to altitude above EKF origin
float Sub::pv_alt_above_origin(float alt_above_home_cm)
{
Location origin;
if (!ahrs.get_origin(origin)) {
origin.zero();
}
return alt_above_home_cm + (ahrs.get_home().alt - origin.alt);
}