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

AR_AttitudeControl: add get_turn_rate_from_heading

Browse Source
This commit is contained in:
Randy Mackay 2019-05-02 15:35:46 +09:00
parent 8907b727f6
commit 6d4d1bc20a
2 changed files with 15 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/APM_Control/AR_AttitudeControl.cpp
View File

@ -302,17 +302,26 @@ float AR_AttitudeControl::get_steering_out_lat_accel(float desired_accel, bool m
// return value is normally in range -1.0 to +1.0 but can be higher or lower // return value is normally in range -1.0 to +1.0 but can be higher or lower
float AR_AttitudeControl::get_steering_out_heading(float heading_rad, float rate_max_rads, bool motor_limit_left, bool motor_limit_right, float dt) float AR_AttitudeControl::get_steering_out_heading(float heading_rad, float rate_max_rads, bool motor_limit_left, bool motor_limit_right, float dt)
{ {
// calculate heading error (in radians) // calculate the desired turn rate (in radians) from the angle error (also in radians)
float desired_rate = get_turn_rate_from_heading(heading_rad, rate_max_rads);
return get_steering_out_rate(desired_rate, motor_limit_left, motor_limit_right, dt);
}
// return a desired turn-rate given a desired heading in radians
float AR_AttitudeControl::get_turn_rate_from_heading(float heading_rad, float rate_max_rads) const
{
const float yaw_error = wrap_PI(heading_rad - _ahrs.yaw); const float yaw_error = wrap_PI(heading_rad - _ahrs.yaw);
// Calculate the desired turn rate (in radians) from the angle error (also in radians) // Calculate the desired turn rate (in radians) from the angle error (also in radians)
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 // limit desired_rate if a custom pivot turn rate is selected, otherwise use ATC_STR_RAT_MAX
if (is_positive(rate_max_rads)) { if (is_positive(rate_max_rads)) {
desired_rate = constrain_float(desired_rate, -rate_max_rads, rate_max_rads); desired_rate = constrain_float(desired_rate, -rate_max_rads, rate_max_rads);
} }
return get_steering_out_rate(desired_rate, motor_limit_left, motor_limit_right, dt); return desired_rate;
} }
// return a steering servo output from -1 to +1 given a // return a steering servo output from -1 to +1 given a

4
libraries/APM_Control/AR_AttitudeControl.h
View File

@ -65,6 +65,10 @@ public:
// return value is normally in range -1.0 to +1.0 but can be higher or lower // return value is normally in range -1.0 to +1.0 but can be higher or lower
float get_steering_out_heading(float heading_rad, float rate_max_rads, bool motor_limit_left, bool motor_limit_right, float dt); float get_steering_out_heading(float heading_rad, float rate_max_rads, bool motor_limit_left, bool motor_limit_right, float dt);
// return a desired turn-rate given a desired heading in radians
// normally the results are later passed into get_steering_out_rate
float get_turn_rate_from_heading(float heading_rad, float rate_max_rads) const;
// return a steering servo output given a desired yaw rate in radians/sec. // return a steering servo output given a desired yaw rate in radians/sec.
// positive yaw is to the right // positive yaw is to the right
// return value is normally in range -1.0 to +1.0 but can be higher or lower // return value is normally in range -1.0 to +1.0 but can be higher or lower