mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-26 03:43:33 +08:00
fxedwingPositionControl: Add slew rate at the end for all mode instead inside each
This commit is contained in:
@@ -502,11 +502,6 @@ void NPFG::updateRollSetpoint()
|
||||
float roll_new = atanf(lateral_accel_ * 1.0f / CONSTANTS_ONE_G);
|
||||
roll_new = math::constrain(roll_new, -roll_lim_rad_, roll_lim_rad_);
|
||||
|
||||
if (dt_ > 0.0f && roll_slew_rate_ > 0.0f) {
|
||||
// slew rate limiting active
|
||||
roll_new = math::constrain(roll_new, roll_setpoint_ - roll_slew_rate_ * dt_, roll_setpoint_ + roll_slew_rate_ * dt_);
|
||||
}
|
||||
|
||||
if (PX4_ISFINITE(roll_new)) {
|
||||
roll_setpoint_ = roll_new;
|
||||
}
|
||||
|
||||
@@ -247,20 +247,6 @@ public:
|
||||
*/
|
||||
void setRollLimit(float roll_lim_rad) { roll_lim_rad_ = roll_lim_rad; }
|
||||
|
||||
/*
|
||||
* [Copied directly from ECL_L1_Pos_Controller]
|
||||
*
|
||||
* Set roll angle slew rate. Set to zero to deactivate.
|
||||
*/
|
||||
void setRollSlewRate(float roll_slew_rate) { roll_slew_rate_ = roll_slew_rate; }
|
||||
|
||||
/*
|
||||
* [Copied directly from ECL_L1_Pos_Controller]
|
||||
*
|
||||
* Set control loop dt. The value will be used to apply roll angle setpoint slew rate limiting.
|
||||
*/
|
||||
void setDt(const float dt) { dt_ = dt; }
|
||||
|
||||
/*
|
||||
* [Copied directly from ECL_L1_Pos_Controller]
|
||||
*
|
||||
@@ -364,10 +350,8 @@ private:
|
||||
* ECL_L1_Pos_Controller functionality
|
||||
*/
|
||||
|
||||
float dt_{0}; // control loop time [s]
|
||||
float roll_lim_rad_{math::radians(30.0f)}; // maximum roll angle [rad]
|
||||
float roll_setpoint_{0.0f}; // current roll angle setpoint [rad]
|
||||
float roll_slew_rate_{0.0f}; // roll angle setpoint slew rate limit [rad/s]
|
||||
|
||||
/*
|
||||
* Adapts the controller period considering user defined inputs, current flight
|
||||
|
||||
Reference in New Issue
Block a user