From 15c95a7b6a81a89dcf3b7c94bcadc741c04f11a4 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 12 Aug 2022 13:35:15 +0200 Subject: [PATCH] FW attitude controller: improve readability and fix euler rate sp vs. body rate sp Signed-off-by: Silvan Fuhrer --- .../FixedwingAttitudeControl.cpp | 30 +++++++++++-------- src/modules/fw_att_control/ecl_controller.cpp | 20 +++++-------- src/modules/fw_att_control/ecl_controller.h | 16 +++++----- .../fw_att_control/ecl_pitch_controller.cpp | 11 +++---- .../fw_att_control/ecl_pitch_controller.h | 2 +- .../fw_att_control/ecl_roll_controller.cpp | 10 ++++--- .../fw_att_control/ecl_wheel_controller.cpp | 21 ++++++------- .../fw_att_control/ecl_yaw_controller.cpp | 19 ++++++------ 8 files changed, 64 insertions(+), 65 deletions(-) diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index e4e64aed48..caef8cfac7 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -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; diff --git a/src/modules/fw_att_control/ecl_controller.cpp b/src/modules/fw_att_control/ecl_controller.cpp index f785319c68..96040eaeda 100644 --- a/src/modules/fw_att_control/ecl_controller.cpp +++ b/src/modules/fw_att_control/ecl_controller.cpp @@ -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() diff --git a/src/modules/fw_att_control/ecl_controller.h b/src/modules/fw_att_control/ecl_controller.h index cb383ccb09..8eda0211ec 100644 --- a/src/modules/fw_att_control/ecl_controller.h +++ b/src/modules/fw_att_control/ecl_controller.h @@ -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); }; diff --git a/src/modules/fw_att_control/ecl_pitch_controller.cpp b/src/modules/fw_att_control/ecl_pitch_controller.cpp index 9b492e45de..9a9055c963 100644 --- a/src/modules/fw_att_control/ecl_pitch_controller.cpp +++ b/src/modules/fw_att_control/ecl_pitch_controller.cpp @@ -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; } diff --git a/src/modules/fw_att_control/ecl_pitch_controller.h b/src/modules/fw_att_control/ecl_pitch_controller.h index 4eaf2e2f84..0b6c8fbca8 100644 --- a/src/modules/fw_att_control/ecl_pitch_controller.h +++ b/src/modules/fw_att_control/ecl_pitch_controller.h @@ -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: diff --git a/src/modules/fw_att_control/ecl_roll_controller.cpp b/src/modules/fw_att_control/ecl_roll_controller.cpp index e63f53574b..e3ae8a628e 100644 --- a/src/modules/fw_att_control/ecl_roll_controller.cpp +++ b/src/modules/fw_att_control/ecl_roll_controller.cpp @@ -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; } diff --git a/src/modules/fw_att_control/ecl_wheel_controller.cpp b/src/modules/fw_att_control/ecl_wheel_controller.cpp index 6220aef428..dd8a3ceae9 100644 --- a/src/modules/fw_att_control/ecl_wheel_controller.cpp +++ b/src/modules/fw_att_control/ecl_wheel_controller.cpp @@ -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; } diff --git a/src/modules/fw_att_control/ecl_yaw_controller.cpp b/src/modules/fw_att_control/ecl_yaw_controller.cpp index 84f105e195..ba0cd5a076 100644 --- a/src/modules/fw_att_control/ecl_yaw_controller.cpp +++ b/src/modules/fw_att_control/ecl_yaw_controller.cpp @@ -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; }