Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-12 01:23:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

AR_AttitudeControl: limit desired steering rate

Browse Source
This commit is contained in:
Ammarf 2018-05-18 16:04:17 +09:00 committed by Randy Mackay
parent abc8208028
commit a9ee949c32
2 changed files with 7 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/APM_Control/AR_AttitudeControl.cpp
View File

@ -203,13 +203,17 @@ float AR_AttitudeControl::get_steering_out_lat_accel(float desired_accel, bool m
}
// return a steering servo output from -1 to +1 given a heading in radians
float AR_AttitudeControl::get_steering_out_heading(float heading_rad, bool motor_limit_left, bool motor_limit_right)
float AR_AttitudeControl::get_steering_out_heading(float heading_rad, float rate_max, bool motor_limit_left, bool motor_limit_right)
{
// calculate heading error (in radians)
const float yaw_error = wrap_PI(heading_rad - _ahrs.yaw);
// Calculate the desired turn rate (in radians) from the angle error (also in radians)
const float desired_rate = _steer_angle_p.get_p(yaw_error);
float desired_rate = _steer_angle_p.get_p(yaw_error);
// limit desired_rate if a custom pivot turn rate is selected, otherwise use ATC_STR_RAT_MAX
if (is_positive(rate_max)) {
desired_rate = constrain_float(desired_rate, -rate_max, rate_max);
}
return get_steering_out_rate(desired_rate, motor_limit_left, motor_limit_right);
}

2
libraries/APM_Control/AR_AttitudeControl.h
View File

@ -48,7 +48,7 @@ public:
float get_steering_out_lat_accel(float desired_accel, bool motor_limit_left, bool motor_limit_right);
// return a steering servo output from -1 to +1 given a heading in radians
float get_steering_out_heading(float heading_rad, bool motor_limit_left, bool motor_limit_right);
float get_steering_out_heading(float heading_rad, float rate_max, bool motor_limit_left, bool motor_limit_right);
// return a steering servo output from -1 to +1 given a
// desired yaw rate in radians/sec. Positive yaw is to the right.