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

copter: fix condition yaw early completion

This commit is contained in:
Iampete1 2022-02-15 17:40:51 +00:00 committed by Randy Mackay
parent 4f915fa5b1
commit 23ea84bf32
2 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/mode.h
View File

@ -234,6 +234,8 @@ public:
void set_yaw_angle_rate(float yaw_angle_d, float yaw_rate_ds);
bool fixed_yaw_slew_finished() { return is_zero(_fixed_yaw_offset_cd); }
private:
float look_ahead_yaw();

8
ArduCopter/mode_auto.cpp
View File

@ -1871,13 +1871,11 @@ bool ModeAuto::verify_within_distance()
// verify_yaw - return true if we have reached the desired heading
bool ModeAuto::verify_yaw()
{
// set yaw mode if it has been changed (the waypoint controller often retakes control of yaw as it executes a new waypoint command)
if (auto_yaw.mode() != AUTO_YAW_FIXED) {
auto_yaw.set_mode(AUTO_YAW_FIXED);
}
// make sure still in fixed yaw mode, the waypoint controller often retakes control of yaw as it executes a new waypoint command
auto_yaw.set_mode(AUTO_YAW_FIXED);
// check if we are within 2 degrees of the target heading
return (fabsf(wrap_180_cd(ahrs.yaw_sensor-auto_yaw.yaw())) <= 200);
return auto_yaw.fixed_yaw_slew_finished() && (fabsf(wrap_180_cd(ahrs.yaw_sensor-auto_yaw.yaw())) <= 200);
}
// verify_nav_wp - check if we have reached the next way point