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

AC_PosControl: always update yaw targets

This commit is contained in:
Randy Mackay 2024-03-23 11:41:51 +09:00
parent efd832425c
commit f28e0fc1d1
2 changed files with 7 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -1231,7 +1231,7 @@ void AC_PosControl::lean_angles_to_accel_xy(float& accel_x_cmss, float& accel_y_
} }
// calculate_yaw_and_rate_yaw - update the calculated the vehicle yaw and rate of yaw. // calculate_yaw_and_rate_yaw - update the calculated the vehicle yaw and rate of yaw.
bool AC_PosControl::calculate_yaw_and_rate_yaw() void AC_PosControl::calculate_yaw_and_rate_yaw()
{ {
// Calculate the turn rate // Calculate the turn rate
float turn_rate = 0.0f; float turn_rate = 0.0f;
@ -1250,9 +1250,12 @@ bool AC_PosControl::calculate_yaw_and_rate_yaw()
if (vel_desired_xy_len > _vel_max_xy_cms * 0.05f) { if (vel_desired_xy_len > _vel_max_xy_cms * 0.05f) {
_yaw_target = degrees(_vel_desired.xy().angle()) * 100.0f; _yaw_target = degrees(_vel_desired.xy().angle()) * 100.0f;
_yaw_rate_target = turn_rate * degrees(100.0f); _yaw_rate_target = turn_rate * degrees(100.0f);
return true; return;
} }
return false;
// use the current attitude controller yaw target
_yaw_target = _attitude_control.get_att_target_euler_cd().z;
_yaw_rate_target = 0;
} }
// calculate_overspeed_gain - calculated increased maximum acceleration and jerk if over speed condition is detected // calculate_overspeed_gain - calculated increased maximum acceleration and jerk if over speed condition is detected

2
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -411,7 +411,7 @@ protected:
void lean_angles_to_accel_xy(float& accel_x_cmss, float& accel_y_cmss) const; void lean_angles_to_accel_xy(float& accel_x_cmss, float& accel_y_cmss) const;
// calculate_yaw_and_rate_yaw - calculate the vehicle yaw and rate of yaw. // calculate_yaw_and_rate_yaw - calculate the vehicle yaw and rate of yaw.
bool calculate_yaw_and_rate_yaw(); void calculate_yaw_and_rate_yaw();
// calculate_overspeed_gain - calculated increased maximum acceleration and jerk if over speed condition is detected // calculate_overspeed_gain - calculated increased maximum acceleration and jerk if over speed condition is detected
float calculate_overspeed_gain(); float calculate_overspeed_gain();