Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-26 09:26:25 +08:00
Code Issues Actions 3 Packages Projects Releases Wiki Activity

FW attitude controller: improve readability and fix euler rate sp vs. body rate sp

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2022-08-12 13:35:15 +02:00
parent e0a998e6ad
commit 15c95a7b6a
8 changed files with 64 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+17 -13
View File
@@ -444,6 +444,9 @@ void FixedwingAttitudeControl::Run()
control_input.roll_setpoint = _att_sp.roll_body;
control_input.pitch_setpoint = _att_sp.pitch_body;
control_input.yaw_setpoint = _att_sp.yaw_body;
control_input.euler_roll_rate_setpoint = _roll_ctrl.get_euler_rate_setpoint();
control_input.euler_pitch_rate_setpoint = _pitch_ctrl.get_euler_rate_setpoint();
control_input.euler_yaw_rate_setpoint = _yaw_ctrl.get_euler_rate_setpoint();
control_input.airspeed_min = _param_fw_airspd_stall.get();
control_input.airspeed_max = _param_fw_airspd_max.get();
control_input.airspeed = airspeed;
@@ -536,12 +539,12 @@ void FixedwingAttitudeControl::Run()
}
/* Update input data for rate controllers */
Vector3f rates_setpoint = Vector3f(_roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate(),
_yaw_ctrl.get_desired_rate());
Vector3f body_rates_setpoint = Vector3f(_roll_ctrl.get_body_rate_setpoint(), _pitch_ctrl.get_body_rate_setpoint(),
_yaw_ctrl.get_body_rate_setpoint());
const hrt_abstime now = hrt_absolute_time();
autotune_attitude_control_status_s pid_autotune;
matrix::Vector3f bodyrate_ff;
matrix::Vector3f bodyrate_autotune_ff;
if (_autotune_attitude_control_status_sub.copy(&pid_autotune)) {
if ((pid_autotune.state == autotune_attitude_control_status_s::STATE_ROLL
@@ -550,14 +553,14 @@ void FixedwingAttitudeControl::Run()
|| pid_autotune.state == autotune_attitude_control_status_s::STATE_TEST)
&& ((now - pid_autotune.timestamp) < 1_s)) {
bodyrate_ff = matrix::Vector3f(pid_autotune.rate_sp);
rates_setpoint = rates_setpoint + bodyrate_ff;
bodyrate_autotune_ff = matrix::Vector3f(pid_autotune.rate_sp);
body_rates_setpoint = body_rates_setpoint + bodyrate_autotune_ff;
}
}
/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
const Vector3f att_control = _rate_control.update(rates, rates_setpoint, angular_accel, dt, _landed);
float roll_feedforward = _param_fw_rr_ff.get() * _airspeed_scaling * rates_setpoint(0);
const Vector3f att_control = _rate_control.update(rates, body_rates_setpoint, angular_accel, dt, _landed);
float roll_feedforward = _param_fw_rr_ff.get() * _airspeed_scaling * body_rates_setpoint(0);
float roll_u = att_control(0) * _airspeed_scaling * _airspeed_scaling + roll_feedforward;
_actuator_controls.control[actuator_controls_s::INDEX_ROLL] =
(PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll;
@@ -566,7 +569,7 @@ void FixedwingAttitudeControl::Run()
_roll_ctrl.reset_integrator();
}
float pitch_feedforward = _param_fw_pr_ff.get() * _airspeed_scaling * rates_setpoint(0);
float pitch_feedforward = _param_fw_pr_ff.get() * _airspeed_scaling * body_rates_setpoint(0);
float pitch_u = att_control(1) * _airspeed_scaling * _airspeed_scaling + pitch_feedforward;
_actuator_controls.control[actuator_controls_s::INDEX_PITCH] =
(PX4_ISFINITE(pitch_u)) ? pitch_u + trim_pitch : trim_pitch;
@@ -623,19 +626,20 @@ void FixedwingAttitudeControl::Run()
* Lazily publish the rate setpoint (for analysis, the actuators are published below)
* only once available
*/
_rates_sp.roll = _roll_ctrl.get_desired_rate();
_rates_sp.pitch = _pitch_ctrl.get_desired_rate();
_rates_sp.yaw = (wheel_control) ? _wheel_ctrl.get_desired_rate() : _yaw_ctrl.get_desired_rate();
_rates_sp.roll = _roll_ctrl.get_body_rate_setpoint();
_rates_sp.pitch = _pitch_ctrl.get_body_rate_setpoint();
_rates_sp.yaw = (wheel_control) ? _wheel_ctrl.get_body_rate_setpoint() : _yaw_ctrl.get_body_rate_setpoint();
_rates_sp.timestamp = hrt_absolute_time();
_rate_sp_pub.publish(_rates_sp);
} else {
// Acro or full manual
vehicle_rates_setpoint_poll();
const Vector3f rates_setpoint = Vector3f(_rates_sp.roll, _rates_sp.pitch, _rates_sp.yaw);
const Vector3f att_control = _rate_control.update(rates, rates_setpoint, angular_accel, dt, _landed);
const Vector3f body_rates_setpoint = Vector3f(_rates_sp.roll, _rates_sp.pitch, _rates_sp.yaw);
const Vector3f att_control = _rate_control.update(rates, body_rates_setpoint, angular_accel, dt, _landed);
_actuator_controls.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(att_control(0))) ? att_control(
0) + trim_roll : trim_roll;
src/modules/fw_att_control/ecl_controller.cpp
+7 -13
View File
@@ -61,9 +61,8 @@ ECL_Controller::ECL_Controller() :
_max_rate(0.0f),
_last_output(0.0f),
_integrator(0.0f),
_rate_error(0.0f),
_rate_setpoint(0.0f),
_bodyrate_setpoint(0.0f)
_euler_rate_setpoint(0.0f),
_body_rate_setpoint(0.0f)
{
}
@@ -106,22 +105,17 @@ void ECL_Controller::set_max_rate(float max_rate)
void ECL_Controller::set_bodyrate_setpoint(float rate)
{
_bodyrate_setpoint = math::constrain(rate, -_max_rate, _max_rate);
_body_rate_setpoint = math::constrain(rate, -_max_rate, _max_rate);
}
float ECL_Controller::get_rate_error()
float ECL_Controller::get_euler_rate_setpoint()
{
return _rate_error;
return _euler_rate_setpoint;
}
float ECL_Controller::get_desired_rate()
float ECL_Controller::get_body_rate_setpoint()
{
return _rate_setpoint;
}
float ECL_Controller::get_desired_bodyrate()
{
return _bodyrate_setpoint;
return _body_rate_setpoint;
}
float ECL_Controller::get_integrator()
src/modules/fw_att_control/ecl_controller.h
+7 -9
View File
@@ -61,9 +61,9 @@ struct ECL_ControlData {
float roll_setpoint;
float pitch_setpoint;
float yaw_setpoint;
float roll_rate_setpoint;
float pitch_rate_setpoint;
float yaw_rate_setpoint;
float euler_roll_rate_setpoint;
float euler_pitch_rate_setpoint;
float euler_yaw_rate_setpoint;
float airspeed_min;
float airspeed_max;
float airspeed;
@@ -90,9 +90,8 @@ public:
void set_bodyrate_setpoint(float rate);
/* Getters */
float get_rate_error();
float get_desired_rate();
float get_desired_bodyrate();
float get_euler_rate_setpoint();
float get_body_rate_setpoint();
float get_integrator();
void reset_integrator();
@@ -107,8 +106,7 @@ protected:
float _max_rate;
float _last_output;
float _integrator;
float _rate_error;
float _rate_setpoint;
float _bodyrate_setpoint;
float _euler_rate_setpoint;
float _body_rate_setpoint;
float constrain_airspeed(float airspeed, float minspeed, float maxspeed);
};
src/modules/fw_att_control/ecl_pitch_controller.cpp
+6 -5
View File
@@ -49,20 +49,21 @@ float ECL_PitchController::control_attitude(const float dt, const ECL_ControlDat
if (!(PX4_ISFINITE(ctl_data.pitch_setpoint) &&
PX4_ISFINITE(ctl_data.roll) &&
PX4_ISFINITE(ctl_data.pitch) &&
PX4_ISFINITE(ctl_data.euler_yaw_rate_setpoint) &&
PX4_ISFINITE(ctl_data.airspeed))) {
return _rate_setpoint;
return _body_rate_setpoint;
}
/* Calculate the error */
float pitch_error = ctl_data.pitch_setpoint - ctl_data.pitch;
/* Apply P controller: rate setpoint from current error and time constant */
float euler_rate_setpoint = pitch_error / _tc;
_euler_rate_setpoint = pitch_error / _tc;
/* Transform setpoint to body angular rates (jacobian) */
_rate_setpoint = cosf(ctl_data.roll) * euler_rate_setpoint +
cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint;
_body_rate_setpoint = cosf(ctl_data.roll) * _euler_rate_setpoint +
cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.euler_yaw_rate_setpoint;
return _rate_setpoint;
return _body_rate_setpoint;
}
src/modules/fw_att_control/ecl_pitch_controller.h
+1 -1
View File
@@ -75,7 +75,7 @@ public:
void set_bodyrate_setpoint(float rate)
{
_bodyrate_setpoint = math::constrain(rate, -_max_rate_neg, _max_rate);
_body_rate_setpoint = math::constrain(rate, -_max_rate_neg, _max_rate);
}
protected:
src/modules/fw_att_control/ecl_roll_controller.cpp
+6 -4
View File
@@ -47,19 +47,21 @@ float ECL_RollController::control_attitude(const float dt, const ECL_ControlData
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.roll_setpoint) &&
PX4_ISFINITE(ctl_data.euler_yaw_rate_setpoint) &&
PX4_ISFINITE(ctl_data.pitch) &&
PX4_ISFINITE(ctl_data.roll))) {
return _rate_setpoint;
return _body_rate_setpoint;
}
/* Calculate the error */
float roll_error = ctl_data.roll_setpoint - ctl_data.roll;
/* Apply P controller: rate setpoint from current error and time constant */
float euler_rate_setpoint = roll_error / _tc;
_euler_rate_setpoint = roll_error / _tc;
/* Transform setpoint to body angular rates (jacobian) */
_rate_setpoint = euler_rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint;
_body_rate_setpoint = _euler_rate_setpoint - sinf(ctl_data.pitch) * ctl_data.euler_yaw_rate_setpoint;
return _rate_setpoint;
return _body_rate_setpoint;
}
src/modules/fw_att_control/ecl_wheel_controller.cpp
+11 -10
View File
@@ -60,11 +60,11 @@ float ECL_WheelController::control_bodyrate(const float dt, const ECL_ControlDat
float min_speed = 1.0f;
/* Calculate body angular rate error */
_rate_error = _rate_setpoint - ctl_data.body_z_rate; //body angular rate error
const float rate_error = _body_rate_setpoint - ctl_data.body_z_rate; //body angular rate error
if (!ctl_data.lock_integrator && _k_i > 0.0f && ctl_data.groundspeed > min_speed) {
float id = _rate_error * dt * ctl_data.groundspeed_scaler;
float id = rate_error * dt * ctl_data.groundspeed_scaler;
/*
* anti-windup: do not allow integrator to increase if actuator is at limit
@@ -83,8 +83,8 @@ float ECL_WheelController::control_bodyrate(const float dt, const ECL_ControlDat
}
/* Apply PI rate controller and store non-limited output */
_last_output = _rate_setpoint * _k_ff * ctl_data.groundspeed_scaler +
ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler * (_rate_error * _k_p + _integrator);
_last_output = _body_rate_setpoint * _k_ff * ctl_data.groundspeed_scaler +
ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler * (rate_error * _k_p + _integrator);
return math::constrain(_last_output, -1.0f, 1.0f);
}
@@ -95,25 +95,26 @@ float ECL_WheelController::control_attitude(const float dt, const ECL_ControlDat
if (!(PX4_ISFINITE(ctl_data.yaw_setpoint) &&
PX4_ISFINITE(ctl_data.yaw))) {
return _rate_setpoint;
return _body_rate_setpoint;
}
/* Calculate the error */
float yaw_error = wrap_pi(ctl_data.yaw_setpoint - ctl_data.yaw);
/* Apply P controller: rate setpoint from current error and time constant */
_rate_setpoint = yaw_error / _tc;
_euler_rate_setpoint = yaw_error / _tc;
_body_rate_setpoint = _euler_rate_setpoint; // assume 0 pitch and roll angle, thus jacobian is simply identity matrix
/* limit the rate */
if (_max_rate > 0.01f) {
if (_rate_setpoint > 0.0f) {
_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
if (_body_rate_setpoint > 0.0f) {
_body_rate_setpoint = (_body_rate_setpoint > _max_rate) ? _max_rate : _body_rate_setpoint;
} else {
_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
_body_rate_setpoint = (_body_rate_setpoint < -_max_rate) ? -_max_rate : _body_rate_setpoint;
}
}
return _rate_setpoint;
return _body_rate_setpoint;
}
src/modules/fw_att_control/ecl_yaw_controller.cpp
+9 -10
View File
@@ -48,10 +48,9 @@ float ECL_YawController::control_attitude(const float dt, const ECL_ControlData
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.roll) &&
PX4_ISFINITE(ctl_data.pitch) &&
PX4_ISFINITE(ctl_data.roll_rate_setpoint) &&
PX4_ISFINITE(ctl_data.pitch_rate_setpoint))) {
PX4_ISFINITE(ctl_data.euler_pitch_rate_setpoint))) {
return _rate_setpoint;
return _body_rate_setpoint;
}
float constrained_roll;
@@ -82,18 +81,18 @@ float ECL_YawController::control_attitude(const float dt, const ECL_ControlData
if (!inverted) {
/* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
float euler_rate_setpoint = tanf(constrained_roll) * cosf(ctl_data.pitch) * CONSTANTS_ONE_G / (ctl_data.airspeed <
ctl_data.airspeed_min ? ctl_data.airspeed_min : ctl_data.airspeed);
_euler_rate_setpoint = tanf(constrained_roll) * cosf(ctl_data.pitch) * CONSTANTS_ONE_G / (ctl_data.airspeed <
ctl_data.airspeed_min ? ctl_data.airspeed_min : ctl_data.airspeed);
/* Transform setpoint to body angular rates (jacobian) */
_rate_setpoint = -sinf(ctl_data.roll) * euler_rate_setpoint +
cosf(ctl_data.roll) * cosf(ctl_data.pitch) * euler_rate_setpoint;
_body_rate_setpoint = -sinf(ctl_data.roll) * ctl_data.euler_pitch_rate_setpoint +
cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _euler_rate_setpoint;
}
if (!PX4_ISFINITE(_rate_setpoint)) {
if (!PX4_ISFINITE(_body_rate_setpoint)) {
PX4_WARN("yaw rate sepoint not finite");
_rate_setpoint = 0.0f;
_body_rate_setpoint = 0.0f;
}
return _rate_setpoint;
return _body_rate_setpoint;
}