Skip to content

Commit

Permalink
Copter: autoyaw: correct units returned by look_ahead_yaw
Browse files Browse the repository at this point in the history
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)
  • Loading branch information
peterbarker committed Apr 11, 2024
1 parent 6b5bb4e commit b849fbb
Show file tree
Hide file tree
Showing 2 changed files with 6 additions and 3 deletions.
7 changes: 4 additions & 3 deletions ArduCopter/autoyaw.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -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:
Expand Down
2 changes: 2 additions & 0 deletions ArduCopter/mode.h
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down

0 comments on commit b849fbb

Please sign in to comment.