From d3e4dd08d1e6a2160fc11e4536cbd757144df242 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 3 Jul 2023 10:55:08 +0900 Subject: [PATCH] AR_AttitudeContol: add steering_limited checks --- libraries/APM_Control/AR_AttitudeControl.cpp | 29 ++++++++++++++++++-- libraries/APM_Control/AR_AttitudeControl.h | 8 ++++++ 2 files changed, 35 insertions(+), 2 deletions(-) diff --git a/libraries/APM_Control/AR_AttitudeControl.cpp b/libraries/APM_Control/AR_AttitudeControl.cpp index 5a3255ea29..f8ed4a3ce5 100644 --- a/libraries/APM_Control/AR_AttitudeControl.cpp +++ b/libraries/APM_Control/AR_AttitudeControl.cpp @@ -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 diff --git a/libraries/APM_Control/AR_AttitudeControl.h b/libraries/APM_Control/AR_AttitudeControl.h index 57d1552659..b3615f76c9 100644 --- a/libraries/APM_Control/AR_AttitudeControl.h +++ b/libraries/APM_Control/AR_AttitudeControl.h @@ -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