mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-23 05:26:38 +08:00
l1 position control: added roll angle setpoint slew rate limiting
Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user