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

Copter: get_bearing & get_horizontal_distance use Vector2f

Browse Source
This commit is contained in:
Josh Henderson 2021-09-10 23:44:02 -04:00 committed by Andrew Tridgell
parent 5fe5d3a3d3
commit 29ebb9be60
3 changed files with 8 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/autoyaw.cpp
View File

@ -5,7 +5,7 @@ Mode::AutoYaw Mode::auto_yaw;
// roi_yaw - returns heading towards location held in roi // roi_yaw - returns heading towards location held in roi
float Mode::AutoYaw::roi_yaw() const float Mode::AutoYaw::roi_yaw() const
{ {
return get_bearing_cd(copter.inertial_nav.get_position(), roi); return get_bearing_cd(copter.inertial_nav.get_position().xy(), roi.xy());
} }
float Mode::AutoYaw::look_ahead_yaw() float Mode::AutoYaw::look_ahead_yaw()

10
ArduCopter/mode_follow.cpp
View File

@ -108,9 +108,8 @@ void ModeFollow::run()
// calculate vehicle heading // calculate vehicle heading
switch (g2.follow.get_yaw_behave()) { switch (g2.follow.get_yaw_behave()) {
case AP_Follow::YAW_BEHAVE_FACE_LEAD_VEHICLE: { case AP_Follow::YAW_BEHAVE_FACE_LEAD_VEHICLE: {
const Vector3f dist_vec_xy(dist_vec.x, dist_vec.y, 0.0f); if (dist_vec.xy().length() > 1.0f) {
if (dist_vec_xy.length() > 1.0f) { yaw_cd = get_bearing_cd(Vector2f{}, dist_vec.xy());
yaw_cd = get_bearing_cd(Vector3f(), dist_vec_xy);
use_yaw = true; use_yaw = true;
} }
break; break;
@ -126,9 +125,8 @@ void ModeFollow::run()
} }
case AP_Follow::YAW_BEHAVE_DIR_OF_FLIGHT: { case AP_Follow::YAW_BEHAVE_DIR_OF_FLIGHT: {
const Vector3f vel_vec(desired_velocity_neu_cms.x, desired_velocity_neu_cms.y, 0.0f); if (desired_velocity_neu_cms.xy().length() > 100.0f) {
if (vel_vec.length() > 100.0f) { yaw_cd = get_bearing_cd(Vector2f{}, desired_velocity_neu_cms.xy());
yaw_cd = get_bearing_cd(Vector3f(), vel_vec);
use_yaw = true; use_yaw = true;
} }
break; break;

6
ArduCopter/mode_guided.cpp
View File

@ -1087,7 +1087,7 @@ bool ModeGuided::limit_check()
// check if we have gone beyond horizontal limit // check if we have gone beyond horizontal limit
if (guided_limit.horiz_max_cm > 0.0f) { if (guided_limit.horiz_max_cm > 0.0f) {
float horiz_move = get_horizontal_distance_cm(guided_limit.start_pos, curr_pos); const float horiz_move = get_horizontal_distance_cm(guided_limit.start_pos.xy(), curr_pos.xy());
if (horiz_move > guided_limit.horiz_max_cm) { if (horiz_move > guided_limit.horiz_max_cm) {
return true; return true;
} }
@ -1118,7 +1118,7 @@ uint32_t ModeGuided::wp_distance() const
case SubMode::WP: case SubMode::WP:
return wp_nav->get_wp_distance_to_destination(); return wp_nav->get_wp_distance_to_destination();
case SubMode::Pos: case SubMode::Pos:
return norm(guided_pos_target_cm.x - inertial_nav.get_position().x, guided_pos_target_cm.y - inertial_nav.get_position().y); return get_horizontal_distance_cm(inertial_nav.get_position().xy(), guided_pos_target_cm.tofloat().xy());
case SubMode::PosVelAccel: case SubMode::PosVelAccel:
return pos_control->get_pos_error_xy_cm(); return pos_control->get_pos_error_xy_cm();
break; break;
@ -1133,7 +1133,7 @@ int32_t ModeGuided::wp_bearing() const
case SubMode::WP: case SubMode::WP:
return wp_nav->get_wp_bearing_to_destination(); return wp_nav->get_wp_bearing_to_destination();
case SubMode::Pos: case SubMode::Pos:
return get_bearing_cd(inertial_nav.get_position(), guided_pos_target_cm.tofloat()); return get_bearing_cd(inertial_nav.get_position().xy(), guided_pos_target_cm.tofloat().xy());
case SubMode::PosVelAccel: case SubMode::PosVelAccel:
return pos_control->get_bearing_to_target_cd(); return pos_control->get_bearing_to_target_cd();
break; break;