fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 19:34:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

l1 position control: added roll angle setpoint slew rate limiting

Browse Source 
Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
Roman 2018-07-23 16:30:08 +02:00 committed by Lorenz Meier
parent 748446b92f
commit fe4e6779f3
2 changed files with 49 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
l1/ecl_l1_pos_controller.cpp
View File

@ -45,11 +45,27 @@
using matrix::Vector2f;
using matrix::wrap_pi;
float ECL_L1_Pos_Controller::nav_roll()
void ECL_L1_Pos_Controller::update_roll_setpoint()
{
float ret = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G);
ret = math::constrain(ret, -_roll_lim_rad, _roll_lim_rad);
return ret;
float roll_new = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G);
roll_new = math::constrain(roll_new, -_roll_lim_rad, _roll_lim_rad);
// no slew rate limiting active
if (_dt <= 0.0f || _roll_slew_rate <= 0.0f) {
_roll_setpoint = roll_new;
return;
}
// slew rate limiting active
if ((roll_new - _roll_setpoint) / _dt > _roll_slew_rate) {
roll_new = _roll_setpoint + _roll_slew_rate * _dt;
} else if ((roll_new - _roll_setpoint) / _dt < -_roll_slew_rate) {
roll_new = _roll_setpoint - _roll_slew_rate * _dt;
}
_roll_setpoint = roll_new;
}
float ECL_L1_Pos_Controller::switch_distance(float wp_radius)
@ -182,6 +198,8 @@ ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector
/* the bearing angle, in NED frame */
_bearing_error = eta;
update_roll_setpoint();
}
void
@ -281,6 +299,8 @@ ECL_L1_Pos_Controller::navigate_loiter(const Vector2f &vector_A, const Vector2f
/* bearing from current position to L1 point */
_nav_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0));
}
update_roll_setpoint();
}
void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading,
@ -316,6 +336,8 @@ void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float cur
/* limit eta to 90 degrees */
eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f);
_lateral_accel = 2.0f * sinf(eta) * omega_vel;
update_roll_setpoint();
}
void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading)
@ -331,6 +353,8 @@ void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading)
/* not circling a waypoint when flying level */
_circle_mode = false;
update_roll_setpoint();
}
Vector2f ECL_L1_Pos_Controller::get_local_planar_vector(const Vector2f &origin, const Vector2f &target) const

23
l1/ecl_l1_pos_controller.h
View File

@ -105,7 +105,7 @@ public:
*
* @return Roll angle (in NED frame)
*/
float nav_roll();
float get_roll_setpoint(){ return _roll_setpoint; }
/**
* Get the current crosstrack error.
@ -195,6 +195,16 @@ public:
*/
void set_l1_roll_limit(float roll_lim_rad) { _roll_lim_rad = roll_lim_rad; }
/**
* Set roll angle slew rate. Set to zero to deactivate.
*/
void set_roll_slew_rate(float roll_slew_rate) { _roll_slew_rate = roll_slew_rate; }
/**
* Set control loop dt. The value will be used to apply roll angle setpoint slew rate limiting.
*/
void set_dt(float dt) { _dt = dt;}
private:
float _lateral_accel{0.0f}; ///< Lateral acceleration setpoint in m/s^2
@ -211,7 +221,10 @@ private:
float _K_L1{2.0f}; ///< L1 control gain for _L1_damping
float _heading_omega{1.0f}; ///< Normalized frequency
float _roll_lim_rad{math::radians(30.0f)}; ///<maximum roll angle
float _roll_lim_rad{math::radians(30.0f)}; ///<maximum roll angle in radians
float _roll_setpoint{0.0f}; ///< current roll angle setpoint in radians
float _roll_slew_rate{0.0f}; ///< roll angle setpoint slew rate limit in rad/s
float _dt{0}; ///< control loop time in seconds
/**
* Convert a 2D vector from WGS84 to planar coordinates.
@ -226,6 +239,12 @@ private:
*/
matrix::Vector2f get_local_planar_vector(const matrix::Vector2f &origin, const matrix::Vector2f &target) const;
/**
* Update roll angle setpoint. This will also apply slew rate limits if set.
*
*/
void update_roll_setpoint();
};