diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index 2e7904498a..92b5685c1a 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp @@ -359,22 +359,22 @@ FixedwingPositionINDIControl::_read_trajectory_coeffs_csv() */ // 100m radius circle trajec - _basis_coeffs_x(0) = 0.000064f; - _basis_coeffs_x(1) = 3020.233571f; - _basis_coeffs_x(2) = -10609.960177f; - _basis_coeffs_x(3) = 17956.458964f; - _basis_coeffs_x(4) = -15735.479961f; - _basis_coeffs_x(5) = 2399.573434f; - _basis_coeffs_x(6) = 11421.854705f; - _basis_coeffs_x(7) = -12388.936542f; - _basis_coeffs_x(8) = -120.944433f; - _basis_coeffs_x(9) = 12530.869640f; - _basis_coeffs_x(10) = -11346.431128f; - _basis_coeffs_x(11) = -2643.369342f; - _basis_coeffs_x(12) = 15999.009519f; - _basis_coeffs_x(13) = -18127.094775f; - _basis_coeffs_x(14) = 10676.696033f; - _basis_coeffs_x(15) = -3032.667571f; + _basis_coeffs_x(0) = -0.000064f; + _basis_coeffs_x(1) = -3020.233571f; + _basis_coeffs_x(2) = 10609.960177f; + _basis_coeffs_x(3) = -17956.458964f; + _basis_coeffs_x(4) = 15735.479961f; + _basis_coeffs_x(5) = -2399.573434f; + _basis_coeffs_x(6) = -11421.854705f; + _basis_coeffs_x(7) = 12388.936542f; + _basis_coeffs_x(8) = 120.944433f; + _basis_coeffs_x(9) = -12530.869640f; + _basis_coeffs_x(10) = 11346.431128f; + _basis_coeffs_x(11) = 2643.369342f; + _basis_coeffs_x(12) = -15999.009519f; + _basis_coeffs_x(13) = 18127.094775f; + _basis_coeffs_x(14) = -10676.696033f; + _basis_coeffs_x(15) = 3032.667571f; _basis_coeffs_y(0) = 100.005984f; _basis_coeffs_y(1) = 4686.100637f; @@ -544,7 +544,7 @@ FixedwingPositionINDIControl::Run() _actuators.control[actuator_controls_s::INDEX_ROLL] = ctrl2(0); _actuators.control[actuator_controls_s::INDEX_PITCH] = ctrl2(1); _actuators.control[actuator_controls_s::INDEX_YAW] = ctrl2(2); - _actuators.control[actuator_controls_s::INDEX_THROTTLE] = 1.0f; + _actuators.control[actuator_controls_s::INDEX_THROTTLE] = 0.5f; _actuators_0_pub.publish(_actuators); //print_message(_actuators); } @@ -809,6 +809,9 @@ FixedwingPositionINDIControl::_compute_NDI_stage_1(Vector3f pos_ref, Vector3f ve // compute angular acceleration command (in body frame) Vector3f rot_acc_command = _K_q*w_err + _K_w*(omega_ref-_omega) + alpha_ref; rot_acc_command = _K_w*(omega_ref-_omega) + alpha_ref; + + // apply LP filtered values for incremental part + return rot_acc_command; } @@ -816,9 +819,39 @@ FixedwingPositionINDIControl::_compute_NDI_stage_1(Vector3f pos_ref, Vector3f ve Vector3f FixedwingPositionINDIControl::_compute_NDI_stage_2(Vector3f ctrl) { + // compute the required body moment to produce the desired body angular acceleration - Vector3f moment = _inertia*ctrl + _omega.cross(_inertia*_omega); - return moment; + Vector3f moment = _inertia*_alpha + _omega.cross(_inertia*_omega); + moment = 1.f*Vector3f{0.1f*_actuators.control[actuator_controls_s::INDEX_ROLL], 1.f*_actuators.control[actuator_controls_s::INDEX_PITCH], 0.1f*_actuators.control[actuator_controls_s::INDEX_YAW]}; + Vector3f moment_filtered = _apply_LP_filter(moment, _m_list, _m_lpf_list); + Vector3f alpha_filtered = _apply_LP_filter(_alpha, _l_list, _l_lpf_list); + Vector3f command = _inertia*(ctrl-alpha_filtered) + moment_filtered; + //command = _inertia*ctrl + _omega.cross(_inertia*_omega); + return command; + + +} + +Vector3f +FixedwingPositionINDIControl::_apply_LP_filter(Vector3f new_input, Vector &old_input, Vector &old_output) +{ + old_input(0) = old_input(1); + old_input(1) = old_input(2); + old_input(2) = new_input; + // + Vector3f output = Vector3f{0.f,0.f,0.f}; + // + output += _a1*old_output(1); + output += _a2*old_output(0); + // + output += _b1*old_input(2); + output += _b2*old_input(1); + output += _b3*old_input(0); + // + old_output(0) = old_output(1); + old_output(1) = output; + // + return output; } Vector3f @@ -826,8 +859,8 @@ FixedwingPositionINDIControl::_compute_actuator_deflections(Vector3f ctrl) { // compute airspeed scaling const float airspeed_constrained = constrain(_airspeed, 5.f, 50.f); - float airspeed_scaling = 1.f/(powf(airspeed_constrained,2)+1.f); - airspeed_scaling = 1.f; + float airspeed_scaling = 20.f/(powf(airspeed_constrained,2)+1.f); + airspeed_scaling = 1.0f; // compute the normalized actuator deflection, including airspeed scaling Vector3f deflection = airspeed_scaling*_K_actuators*ctrl; diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp index 4516757308..8d625773ea 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp @@ -16,10 +16,12 @@ #include #include #include +#include "fw_att_control/ecl_pitch_controller.h" +#include "fw_att_control/ecl_roll_controller.h" +#include "fw_att_control/ecl_wheel_controller.h" +#include "fw_att_control/ecl_yaw_controller.h" #include #include -#include -#include #include #include #include @@ -237,6 +239,7 @@ private: Quatf _get_attitude(Vector3f vel, Vector3f f); // get the attitude to produce force f while flying with velocity vel Vector3f _compute_NDI_stage_1(Vector3f pos_ref, Vector3f vel_ref, Vector3f acc_ref, Vector3f omega_ref, Vector3f alpha_ref); Vector3f _compute_NDI_stage_2(Vector3f ctrl); + Vector3f _apply_LP_filter(Vector3f new_input, Vector &old_input, Vector &old_output); Vector3f _compute_actuator_deflections(Vector3f ctrl); // control variables @@ -260,10 +263,14 @@ private: hrt_abstime _last_run{0}; // filter variables - std::array _f_list; - std::array _a_list; - std::array _f_lpf_list; - std::array _a_lpf_list; + Vector _f_list; // force + Vector _m_list; // moment + Vector _a_list; // linear accel + Vector _l_list; // angular accel + Vector _f_lpf_list; + Vector _m_lpf_list; + Vector _a_lpf_list; + Vector _l_lpf_list; // parameter variables Matrix3f _inertia {}; @@ -281,6 +288,11 @@ private: float _b2; float _b3; + // body rate controllers + ECL_RollController _roll_ctrl; + ECL_PitchController _pitch_ctrl; + ECL_YawController _yaw_ctrl; + bool _airspeed_valid{false}; ///< flag if a valid airspeed estimate exists hrt_abstime _airspeed_last_valid{0}; ///< last time airspeed was received. Used to detect timeouts. float _airspeed{0.0f}; diff --git a/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c b/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c index 4e1f0fe63f..0937ffaa0d 100644 --- a/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c +++ b/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c @@ -169,6 +169,17 @@ PARAM_DEFINE_FLOAT(C_D1, 0.3783f); */ PARAM_DEFINE_FLOAT(C_D2, 1.984f); +/** + * coefficients of the butterworth filter used for smoothing the IMU + * + * @unit + * @min -100 + * @max 100 + * @decimal 4 + * @increment 0.0001 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(FILTER_A1, 1.82292669f); /** * coefficients of the butterworth filter used for smoothing the IMU @@ -180,7 +191,7 @@ PARAM_DEFINE_FLOAT(C_D2, 1.984f); * @increment 0.0001 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(FILTER_A1, 0.0f); +PARAM_DEFINE_FLOAT(FILTER_A2, -0.83737699f); /** * coefficients of the butterworth filter used for smoothing the IMU @@ -192,7 +203,7 @@ PARAM_DEFINE_FLOAT(FILTER_A1, 0.0f); * @increment 0.0001 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(FILTER_A2, 0.0f); +PARAM_DEFINE_FLOAT(FILTER_B1, 0.00361257f); /** * coefficients of the butterworth filter used for smoothing the IMU @@ -204,7 +215,7 @@ PARAM_DEFINE_FLOAT(FILTER_A2, 0.0f); * @increment 0.0001 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(FILTER_B1, 0.0f); +PARAM_DEFINE_FLOAT(FILTER_B2, 0.00722515f); /** * coefficients of the butterworth filter used for smoothing the IMU @@ -216,19 +227,7 @@ PARAM_DEFINE_FLOAT(FILTER_B1, 0.0f); * @increment 0.0001 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(FILTER_B2, 0.0f); - -/** - * coefficients of the butterworth filter used for smoothing the IMU - * - * @unit - * @min -100 - * @max 100 - * @decimal 4 - * @increment 0.0001 - * @group FW DYN SOAR Control - */ -PARAM_DEFINE_FLOAT(FILTER_B3, 0.0f); +PARAM_DEFINE_FLOAT(FILTER_B3, 0.00361257f); // ======================================================== // =================== CONTROL GAINS ====================== @@ -423,7 +422,7 @@ PARAM_DEFINE_FLOAT(K_W_YAW, 5.0f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(K_ACT_ROLL, 0.1f); +PARAM_DEFINE_FLOAT(K_ACT_ROLL, 1.0f); /** * pitch gain of K_ACT (actuator deflection gain) @@ -435,7 +434,7 @@ PARAM_DEFINE_FLOAT(K_ACT_ROLL, 0.1f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(K_ACT_PITCH, 0.1f); +PARAM_DEFINE_FLOAT(K_ACT_PITCH, 1.0f); /** * yaw gain of K_ACT (actuator deflection gain) @@ -447,4 +446,4 @@ PARAM_DEFINE_FLOAT(K_ACT_PITCH, 0.1f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(K_ACT_YAW, 0.1f); +PARAM_DEFINE_FLOAT(K_ACT_YAW, 1.0f);