Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: get_bearing & get_horizontal_distance use Vector2f

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
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()

10
ArduCopter/mode_follow.cpp
View File

@ -108,9 +108,8 @@ void ModeFollow::run()
// calculate vehicle heading
switch (g2.follow.get_yaw_behave()) {
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) {
yaw_cd = get_bearing_cd(Vector3f(), dist_vec_xy);
if (dist_vec.xy().length() > 1.0f) {
yaw_cd = get_bearing_cd(Vector2f{}, dist_vec.xy());
use_yaw = true;
}
break;
@ -126,9 +125,8 @@ void ModeFollow::run()
}
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 (vel_vec.length() > 100.0f) {
yaw_cd = get_bearing_cd(Vector3f(), vel_vec);
if (desired_velocity_neu_cms.xy().length() > 100.0f) {
yaw_cd = get_bearing_cd(Vector2f{}, desired_velocity_neu_cms.xy());
use_yaw = true;
}
break;

6
ArduCopter/mode_guided.cpp
View File

@ -1087,7 +1087,7 @@ bool ModeGuided::limit_check()
// check if we have gone beyond horizontal limit
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) {
return true;
}
@ -1118,7 +1118,7 @@ uint32_t ModeGuided::wp_distance() const
case SubMode::WP:
return wp_nav->get_wp_distance_to_destination();
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:
return pos_control->get_pos_error_xy_cm();
break;
@ -1133,7 +1133,7 @@ int32_t ModeGuided::wp_bearing() const
case SubMode::WP:
return wp_nav->get_wp_bearing_to_destination();
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:
return pos_control->get_bearing_to_target_cd();
break;