From b849fbbcca616ca57b349f254d3765d7d723966d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 9 Apr 2024 16:48:42 +1000 Subject: [PATCH] Copter: autoyaw: correct units returned by look_ahead_yaw there are other methods on the autoyaw object which make it clear that they're working in cd, and others in there that work in degrees. This method doesn't specify cd yet returns in that unit. Change the method and state variable to store in degrees (as our naming standards suggest) --- ArduCopter/autoyaw.cpp | 7 ++++--- ArduCopter/mode.h | 2 ++ 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/ArduCopter/autoyaw.cpp b/ArduCopter/autoyaw.cpp index 749a830f4dce6..f7042b4406cf8 100644 --- a/ArduCopter/autoyaw.cpp +++ b/ArduCopter/autoyaw.cpp @@ -8,13 +8,14 @@ float Mode::AutoYaw::roi_yaw() const return get_bearing_cd(copter.inertial_nav.get_position_xy_cm(), roi.xy()); } +// returns a yaw in degrees, direction of vehicle travel: float Mode::AutoYaw::look_ahead_yaw() { const Vector3f& vel = copter.inertial_nav.get_velocity_neu_cms(); const float speed_sq = vel.xy().length_squared(); // Commanded Yaw to automatically look ahead. if (copter.position_ok() && (speed_sq > (YAW_LOOK_AHEAD_MIN_SPEED * YAW_LOOK_AHEAD_MIN_SPEED))) { - _look_ahead_yaw = degrees(atan2f(vel.y,vel.x))*100.0f; + _look_ahead_yaw = degrees(atan2f(vel.y,vel.x)); } return _look_ahead_yaw; } @@ -80,7 +81,7 @@ void Mode::AutoYaw::set_mode(Mode yaw_mode) case Mode::LOOK_AHEAD: // Commanded Yaw to automatically look ahead. - _look_ahead_yaw = copter.ahrs.yaw_sensor; + _look_ahead_yaw = copter.ahrs.yaw_sensor * 0.01; // cdeg -> deg break; case Mode::RESETTOARMEDYAW: @@ -230,7 +231,7 @@ float Mode::AutoYaw::yaw_cd() case Mode::LOOK_AHEAD: // Commanded Yaw to automatically look ahead. - _yaw_angle_cd = look_ahead_yaw(); + _yaw_angle_cd = look_ahead_yaw() * 100.0; break; case Mode::RESETTOARMEDYAW: diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 2e33f121c3026..ad0618e45df13 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -341,7 +341,9 @@ class Mode { // rate_cds(): desired yaw rate in centidegrees/second: float rate_cds(); + // returns a yaw in degrees, direction of vehicle travel: float look_ahead_yaw(); + float roi_yaw() const; // auto flight mode's yaw mode