l1 position control: added roll angle setpoint slew rate limiting

Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
Roman
2018-07-26 07:54:38 +02:00
committed by Lorenz Meier
parent 748446b92f
commit fe4e6779f3
2 changed files with 49 additions and 6 deletions
+28 -4
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