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

AR_AttitudeContol: add steering_limited checks

This commit is contained in:
Randy Mackay 2023-07-03 10:55:08 +09:00
parent 46702e6c49
commit 6f52eaf9a7
2 changed files with 35 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
libraries/APM_Control/AR_AttitudeControl.cpp
View File

@ -529,13 +529,19 @@ float AR_AttitudeControl::get_turn_rate_from_heading(float heading_rad, float ra
return desired_rate;
}
// return a steering servo output from -1 to +1 given a
// desired yaw rate in radians/sec. Positive yaw is to the right.
// return a steering servo output given a desired yaw rate in radians/sec.
// positive yaw is to the right
// return value is normally in range -1.0 to +1.0 but can be higher or lower
// also sets steering_limit_left and steering_limit_right flags
float AR_AttitudeControl::get_steering_out_rate(float desired_rate, bool motor_limit_left, bool motor_limit_right, float dt)
{
// sanity check dt
dt = constrain_float(dt, 0.0f, 1.0f);
// update steering limit flags used by higher level controllers (e.g. position controller)
_steering_limit_left = motor_limit_left;
_steering_limit_right = motor_limit_right;
// if not called recently, reset input filter and desired turn rate to actual turn rate (used for accel limiting)
const uint32_t now = AP_HAL::millis();
if ((_steer_turn_last_ms == 0) || ((now - _steer_turn_last_ms) > AR_ATTCONTROL_TIMEOUT_MS)) {
@ -548,6 +554,12 @@ float AR_AttitudeControl::get_steering_out_rate(float desired_rate, bool motor_l
// acceleration limit desired turn rate
if (is_positive(_steer_accel_max)) {
const float change_max = radians(_steer_accel_max) * dt;
if (desired_rate <= _desired_turn_rate - change_max) {
_steering_limit_left = true;
}
if (desired_rate >= _desired_turn_rate + change_max) {
_steering_limit_right = true;
}
desired_rate = constrain_float(desired_rate, _desired_turn_rate - change_max, _desired_turn_rate + change_max);
}
_desired_turn_rate = desired_rate;
@ -555,6 +567,12 @@ float AR_AttitudeControl::get_steering_out_rate(float desired_rate, bool motor_l
// rate limit desired turn rate
if (is_positive(_steer_rate_max)) {
const float steer_rate_max_rad = radians(_steer_rate_max);
if (_desired_turn_rate <= -steer_rate_max_rad) {
_steering_limit_left = true;
}
if (_desired_turn_rate >= steer_rate_max_rad) {
_steering_limit_right = true;
}
_desired_turn_rate = constrain_float(_desired_turn_rate, -steer_rate_max_rad, steer_rate_max_rad);
}
@ -563,9 +581,16 @@ float AR_AttitudeControl::get_steering_out_rate(float desired_rate, bool motor_l
if (get_forward_speed(speed)) {
// do not limit to less than 1 deg/s
const float turn_rate_max = MAX(get_turn_rate_from_lat_accel(get_turn_lat_accel_max(), fabsf(speed)), radians(1.0f));
if (_desired_turn_rate <= -turn_rate_max) {
_steering_limit_left = true;
}
if (_desired_turn_rate >= turn_rate_max) {
_steering_limit_right = true;
}
_desired_turn_rate = constrain_float(_desired_turn_rate, -turn_rate_max, turn_rate_max);
}
// update pid to calculate output to motors
float output = _steer_rate_pid.update_all(_desired_turn_rate, AP::ahrs().get_yaw_rate_earth(), dt, (motor_limit_left || motor_limit_right));
output += _steer_rate_pid.get_ff();
// constrain and return final output

8
libraries/APM_Control/AR_AttitudeControl.h
View File

@ -31,6 +31,7 @@ public:
// return a steering servo output given a desired yaw rate in radians/sec.
// positive yaw is to the right
// return value is normally in range -1.0 to +1.0 but can be higher or lower
// also sets steering_limit_left and steering_limit_right flags
float get_steering_out_rate(float desired_rate, bool motor_limit_left, bool motor_limit_right, float dt);
// get latest desired turn rate in rad/sec recorded during calls to get_steering_out_rate. For reporting purposes only
@ -48,6 +49,11 @@ public:
// get the lateral acceleration limit (in m/s/s). Returns at least 0.1G or approximately 1 m/s/s
float get_turn_lat_accel_max() const { return MAX(_turn_lateral_G_max, 0.1f) * GRAVITY_MSS; }
// returns true if the steering has been limited which can be caused by the physical steering surface
// reaching its physical limits (aka motor limits) or acceleration or turn rate limits being applied
bool steering_limit_left() const { return _steering_limit_left; }
bool steering_limit_right() const { return _steering_limit_right; }
//
// throttle / speed controller
//
@ -143,6 +149,8 @@ private:
uint32_t _steer_turn_last_ms; // system time of last call to steering rate controller
float _desired_lat_accel; // desired lateral acceleration (in m/s/s) from latest call to get_steering_out_lat_accel (for reporting purposes)
float _desired_turn_rate; // desired turn rate (in radians/sec) either from external caller or from lateral acceleration controller
bool _steering_limit_left; // true when the steering control has reached its left limit (e.g. motor has reached limits or accel or turn rate limits applied)
bool _steering_limit_right; // true when the steering control has reached its right limit (e.g. motor has reached limits or accel or turn rate limits applied)
// throttle control
uint32_t _speed_last_ms; // system time of last call to get_throttle_out_speed