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

Copter: autoyaw: correct units returned by look_ahead_yaw

Browse Source 
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)
This commit is contained in:
Peter Barker 2024-04-09 16:48:42 +10:00 committed by Peter Barker
parent 6b5bb4e864
commit b849fbbcca
2 changed files with 6 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduCopter/autoyaw.cpp
View File

@ -8,13 +8,14 @@ float Mode::AutoYaw::roi_yaw() const
return get_bearing_cd(copter.inertial_nav.get_position_xy_cm(), roi.xy()); 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() float Mode::AutoYaw::look_ahead_yaw()
{ {
const Vector3f& vel = copter.inertial_nav.get_velocity_neu_cms(); const Vector3f& vel = copter.inertial_nav.get_velocity_neu_cms();
const float speed_sq = vel.xy().length_squared(); const float speed_sq = vel.xy().length_squared();
// Commanded Yaw to automatically look ahead. // Commanded Yaw to automatically look ahead.
if (copter.position_ok() && (speed_sq > (YAW_LOOK_AHEAD_MIN_SPEED * YAW_LOOK_AHEAD_MIN_SPEED))) { 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; return _look_ahead_yaw;
} }
@ -80,7 +81,7 @@ void Mode::AutoYaw::set_mode(Mode yaw_mode)
case Mode::LOOK_AHEAD: case Mode::LOOK_AHEAD:
// Commanded Yaw to automatically 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; break;
case Mode::RESETTOARMEDYAW: case Mode::RESETTOARMEDYAW:
@ -230,7 +231,7 @@ float Mode::AutoYaw::yaw_cd()
case Mode::LOOK_AHEAD: case Mode::LOOK_AHEAD:
// Commanded Yaw to automatically look ahead. // Commanded Yaw to automatically look ahead.
_yaw_angle_cd = look_ahead_yaw(); _yaw_angle_cd = look_ahead_yaw() * 100.0;
break; break;
case Mode::RESETTOARMEDYAW: case Mode::RESETTOARMEDYAW:

2
ArduCopter/mode.h
View File

@ -341,7 +341,9 @@ public:
// rate_cds(): desired yaw rate in centidegrees/second: // rate_cds(): desired yaw rate in centidegrees/second:
float rate_cds(); float rate_cds();
// returns a yaw in degrees, direction of vehicle travel:
float look_ahead_yaw(); float look_ahead_yaw();
float roi_yaw() const; float roi_yaw() const;
// auto flight mode's yaw mode // auto flight mode's yaw mode