mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-16 18:33:34 +08:00
added parameter for maximal roll angle
This commit is contained in:
@@ -43,7 +43,7 @@
|
||||
float ECL_L1_Pos_Controller::nav_roll()
|
||||
{
|
||||
float ret = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G);
|
||||
ret = math::constrain(ret, (-M_PI_F) / 2.0f, M_PI_F / 2.0f);
|
||||
ret = math::constrain(ret, -_roll_lim_rad, _roll_lim_rad);
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
@@ -222,6 +222,15 @@ public:
|
||||
_K_L1 = 4.0f * _L1_damping * _L1_damping;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Set the maximum roll angle output in radians
|
||||
*
|
||||
*/
|
||||
void set_l1_roll_limit(float roll_lim_rad) {
|
||||
_roll_lim_rad = roll_lim_rad;
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
float _lateral_accel; ///< Lateral acceleration setpoint in m/s^2
|
||||
@@ -238,6 +247,8 @@ private:
|
||||
float _K_L1; ///< L1 control gain for _L1_damping
|
||||
float _heading_omega; ///< Normalized frequency
|
||||
|
||||
float _roll_lim_rad; ///<maximum roll angle
|
||||
|
||||
/**
|
||||
* Convert a 2D vector from WGS84 to planar coordinates.
|
||||
*
|
||||
|
||||
Reference in New Issue
Block a user