Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: inav use _xy()

Browse Source
This commit is contained in:
Josh Henderson 2021-09-11 04:10:00 -04:00 committed by Andrew Tridgell
parent c6dd39773d
commit 17243b5630
3 changed files with 4 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduSub/Attitude.cpp
View File

@ -64,7 +64,7 @@ float Sub::get_roi_yaw()
roi_yaw_counter++; roi_yaw_counter++;
if (roi_yaw_counter >= 4) { if (roi_yaw_counter >= 4) {
roi_yaw_counter = 0; roi_yaw_counter = 0;
yaw_look_at_WP_bearing = get_bearing_cd(inertial_nav.get_position().xy(), roi_WP.xy()); yaw_look_at_WP_bearing = get_bearing_cd(inertial_nav.get_position_xy(), roi_WP.xy());
} }
return yaw_look_at_WP_bearing; return yaw_look_at_WP_bearing;

6
ArduSub/commands_logic.cpp
View File

@ -509,18 +509,16 @@ bool Sub::verify_circle(const AP_Mission::Mission_Command& cmd)
// check if we've reached the edge // check if we've reached the edge
if (auto_mode == Auto_CircleMoveToEdge) { if (auto_mode == Auto_CircleMoveToEdge) {
if (wp_nav.reached_wp_destination()) { if (wp_nav.reached_wp_destination()) {
Vector3f curr_pos = inertial_nav.get_position();
Vector3f circle_center = pv_location_to_vector(cmd.content.location); Vector3f circle_center = pv_location_to_vector(cmd.content.location);
// set target altitude if not provided // set target altitude if not provided
if (is_zero(circle_center.z)) { if (is_zero(circle_center.z)) {
circle_center.z = curr_pos.z; circle_center.z = inertial_nav.get_altitude();
} }
// set lat/lon position if not provided // set lat/lon position if not provided
if (cmd.content.location.lat == 0 && cmd.content.location.lng == 0) { if (cmd.content.location.lat == 0 && cmd.content.location.lng == 0) {
circle_center.x = curr_pos.x; circle_center.xy() = inertial_nav.get_position_xy();
circle_center.y = curr_pos.y;
} }
// start circling // start circling

4
ArduSub/control_auto.cpp
View File

@ -206,9 +206,7 @@ void Sub::auto_circle_movetoedge_start(const Location &circle_center, float radi
} }
// if we are outside the circle, point at the edge, otherwise hold yaw // if we are outside the circle, point at the edge, otherwise hold yaw
const Vector3p &circle_center_neu = circle_nav.get_center(); float dist_to_center = get_horizontal_distance_cm(inertial_nav.get_position_xy().topostype(), circle_nav.get_center().xy());
const Vector3f &curr_pos = inertial_nav.get_position();
float dist_to_center = norm(circle_center_neu.x - curr_pos.x, circle_center_neu.y - curr_pos.y);
if (dist_to_center > circle_nav.get_radius() && dist_to_center > 500) { if (dist_to_center > circle_nav.get_radius() && dist_to_center > 500) {
set_auto_yaw_mode(get_default_auto_yaw_mode(false)); set_auto_yaw_mode(get_default_auto_yaw_mode(false));
} else { } else {