From 980c41e273f0664cb1fcbb049848bee303786d07 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sun, 2 May 2021 17:36:32 +0100 Subject: [PATCH] APM_Control: G limit all turn rates --- libraries/APM_Control/AR_AttitudeControl.cpp | 17 +++++++++++++++++ libraries/APM_Control/AR_AttitudeControl.h | 4 ++++ 2 files changed, 21 insertions(+) diff --git a/libraries/APM_Control/AR_AttitudeControl.cpp b/libraries/APM_Control/AR_AttitudeControl.cpp index 28fc81e082..3b62baf92a 100644 --- a/libraries/APM_Control/AR_AttitudeControl.cpp +++ b/libraries/APM_Control/AR_AttitudeControl.cpp @@ -394,6 +394,15 @@ const AP_Param::GroupInfo AR_AttitudeControl::var_info[] = { AP_SUBGROUPINFO(_sailboat_heel_pid, "_SAIL_", 12, AR_AttitudeControl, AC_PID), + // @Param: _TURN_MAX_G + // @DisplayName: Turning maximum G force + // @Description: The maximum turning acceleration (in units of gravities) that the rover can handle while remaining stable. The navigation code will keep the lateral acceleration below this level to avoid rolling over or slipping the wheels in turns + // @Units: gravities + // @Range: 0.1 10 + // @Increment: 0.01 + // @User: Standard + AP_GROUPINFO("_TURN_MAX_G", 13, AR_AttitudeControl, _turn_lateral_G_max, 0.6f), + AP_GROUPEND }; @@ -485,6 +494,14 @@ float AR_AttitudeControl::get_steering_out_rate(float desired_rate, bool motor_l _desired_turn_rate = constrain_float(_desired_turn_rate, -steer_rate_max_rad, steer_rate_max_rad); } + // G limit based on speed + float speed; + 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(), speed), radians(1.0f)); + _desired_turn_rate = constrain_float(_desired_turn_rate, -turn_rate_max, turn_rate_max); + } + // set PID's dt _steer_rate_pid.set_dt(dt); diff --git a/libraries/APM_Control/AR_AttitudeControl.h b/libraries/APM_Control/AR_AttitudeControl.h index af9c2fb314..84b0ea975e 100644 --- a/libraries/APM_Control/AR_AttitudeControl.h +++ b/libraries/APM_Control/AR_AttitudeControl.h @@ -86,6 +86,9 @@ public: // calculate the turn rate in rad/sec given a lateral acceleration (in m/s/s) and speed (in m/s) float get_turn_rate_from_lat_accel(float lat_accel, float speed) const; + // get the G limit lateral acceleration ( m/s/s), returning at least 0.1G + float get_turn_lat_accel_max() const { return MAX(_turn_lateral_G_max, 0.1f) * GRAVITY_MSS; } + // // throttle / speed controller // @@ -167,6 +170,7 @@ private: AP_Float _stop_speed; // speed control stop speed. Motor outputs to zero once vehicle speed falls below this value AP_Float _steer_accel_max; // steering angle acceleration max in deg/s/s AP_Float _steer_rate_max; // steering rate control maximum rate in deg/s + AP_Float _turn_lateral_G_max; // sterring maximum lateral acceleration limit in 'G' // steering control uint32_t _steer_lat_accel_last_ms; // system time of last call to lateral acceleration controller (i.e. get_steering_out_lat_accel)