From 93b2cd1cd8e761383134884e4183f84107824a9f Mon Sep 17 00:00:00 2001 From: Marvin Harms Date: Thu, 5 May 2022 14:53:50 +0200 Subject: [PATCH] changed incremental filter settings --- .../FixedwingPositionINDIControl.cpp | 83 ++++++++++++++----- .../FixedwingPositionINDIControl.hpp | 15 ++++ .../fw_dyn_soar_control_params.c | 48 +++++------ .../imu_gyro_parameters.c | 2 +- 4 files changed, 103 insertions(+), 45 deletions(-) diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index e1c58cf9cc..a9dc33daeb 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp @@ -85,6 +85,7 @@ FixedwingPositionINDIControl::init() _R_ned_to_enu(2,2) = -1.f; _R_ned_to_enu.renormalize(); _R_enu_to_ned = _R_ned_to_enu; + return true; } @@ -306,6 +307,12 @@ FixedwingPositionINDIControl::vehicle_local_position_poll() } } +void +FixedwingPositionINDIControl::actuator_controls_poll() +{ + _actuator_controls_sub.update(&_actuators); +} + void FixedwingPositionINDIControl::soaring_controller_status_poll() { @@ -496,6 +503,11 @@ FixedwingPositionINDIControl::Run() vehicle_angular_acceleration_poll(); soaring_controller_status_poll(); + // only run actuators poll, when our module is not publishing: + if (_vehicle_status.nav_state != vehicle_status_s::NAVIGATION_STATE_OFFBOARD) { + actuator_controls_poll(); + } + // ============================ // compute reference kinematics @@ -524,6 +536,12 @@ FixedwingPositionINDIControl::Run() // compute control input // ===================== Vector3f ctrl = _compute_NDI_stage_1(pos_ref, vel_ref, acc_ref, omega_ref, alpha_ref); + Vector3f ctrl1 = _compute_NDI_stage_2(ctrl); + + // ============================ + // compute actuator deflections + // ============================ + Vector3f ctrl2 = _compute_actuator_deflections(ctrl1); // ================================= // publish offboard control commands @@ -569,12 +587,6 @@ FixedwingPositionINDIControl::Run() _angular_vel_sp.pitch = omega_ref(1); _angular_vel_sp.yaw = omega_ref(2); _angular_vel_sp_pub.publish(_angular_vel_sp); - - // ============================ - // compute actuator deflections - // ============================ - Vector3f ctrl1 = _compute_NDI_stage_2(ctrl); - Vector3f ctrl2 = _compute_actuator_deflections(ctrl1); // ========================= // publish acutator controls @@ -588,6 +600,15 @@ FixedwingPositionINDIControl::Run() _actuators.control[actuator_controls_s::INDEX_THROTTLE] = 0.8f; _actuators_0_pub.publish(_actuators); //print_message(_actuators); + + if (_counter==100) { + _counter = 0; + PX4_INFO("frequency: \t%.3f", (double)(1000000*100)/(hrt_absolute_time()-_last_time)); + _last_time = hrt_absolute_time(); + } + else { + _counter += 1; + } } // =========================== @@ -857,10 +878,14 @@ FixedwingPositionINDIControl::_compute_NDI_stage_1(Vector3f pos_ref, Vector3f ve { Dcmf R_ib(_att); Dcmf R_bi(R_ib.transpose()); + // apply LP filter to acceleration + Vector3f acc_filtered; + acc_filtered(0) = _lp_filter_accel[0].apply(_acc(0)); + acc_filtered(1) = _lp_filter_accel[1].apply(_acc(1)); + acc_filtered(2) = _lp_filter_accel[2].apply(_acc(2)); // ========================================= // apply PD control law on the body position // ========================================= - Vector3f acc_filtered = _apply_LP_filter(_acc, _a_list, _a_lpf_list); Vector3f acc_command = R_ib*(_K_x*R_bi*(pos_ref-_pos) + _K_v*R_bi*(vel_ref-_vel) + _K_a*R_bi*(acc_ref-acc_filtered)) + acc_ref; // add gravity acc_command(2) += 9.81f; @@ -877,12 +902,16 @@ FixedwingPositionINDIControl::_compute_NDI_stage_1(Vector3f pos_ref, Vector3f ve Vector3f w_x = vel_body; Vector3f w_z = w_x.cross(Vector3f{0.f,1.f,0.f}); f_current = R_ib*(factor*(C_l*w_z + C_d*w_x)) + Vector3f{0.f,0.f,-_mass*9.81f}; - Vector3f f_current_filtered = _apply_LP_filter(f_current, _f_list, _f_lpf_list); + // apply LP filter to force + Vector3f f_current_filtered; + f_current_filtered(0) = _lp_filter_force[0].apply(f_current(0)); + f_current_filtered(1) = _lp_filter_force[1].apply(f_current(1)); + f_current_filtered(2) = _lp_filter_force[2].apply(f_current(2)); // =============================== // get force comand in world frame // =============================== - Vector3f f_command = _mass*(acc_command - acc_filtered) + f_current_filtered;//_mass*acc_command; + Vector3f f_command = _mass*(acc_command - acc_filtered) + f_current_filtered; // ========================================================================== // get required attitude (assuming we can fly the target velocity), and error @@ -897,9 +926,12 @@ FixedwingPositionINDIControl::_compute_NDI_stage_1(Vector3f pos_ref, Vector3f ve // ========================================= // apply PD control law on the body attitude // ========================================= - Vector3f omega_filtered = _apply_LP_filter(_omega, _w_list, _w_lpf_list); + Vector3f omega_filtered; + omega_filtered(0) = _lp_filter_omega[0].apply(_omega(0)); + omega_filtered(1) = _lp_filter_omega[1].apply(_omega(1)); + omega_filtered(2) = _lp_filter_omega[2].apply(_omega(2)); Vector3f rot_acc_command = _K_q*w_err + _K_w*(omega_ref-omega_filtered) + alpha_ref; - + //rot_acc_command = Vector3f{-1.0f,0.f,0.f}; //PX4_INFO("force command: \t%.2f\t%.2f\t%.2f", (double)f_command(0), (double)f_command(1), (double)f_command(2)); //PX4_INFO("FRD body frame rotation vec: \t%.2f\t%.2f\t%.2f", (double)w_err(0), (double)w_err(1), (double)w_err(2)); @@ -913,33 +945,43 @@ FixedwingPositionINDIControl::_compute_NDI_stage_2(Vector3f ctrl) float c_ail = _param_k_act_roll.get(); float c_ele = _param_k_act_pitch.get(); float c_rud = _param_k_act_yaw.get(); + float c_damping = 0.0f; // compute velocity in body frame Dcmf R_ib(_att); Vector3f vel_body = R_ib.transpose()*_vel; + float q = 0.5f*sqrtf(vel_body*vel_body)*vel_body(0); // dynamic pressure //Vector3f vel_body_2 = Dcmf(Quatf(_attitude.q)).transpose()*Vector3f{_local_pos.vx,_local_pos.vy,_local_pos.vz}; //PX4_INFO("ENU body frame velocity: \t%.2f\t%.2f\t%.2f", (double)vel_body_2(0), (double)vel_body_2(1), (double)vel_body_2(2)); //PX4_INFO("FRD body frame velocity: \t%.2f\t%.2f\t%.2f", (double)vel_body(0), (double)vel_body(1), (double)vel_body(2)); // compute moments Vector3f moment; - moment(0) = 0.5f*c_ail*sqrtf(vel_body*vel_body)*vel_body(0)*_actuators.control[actuator_controls_s::INDEX_ROLL]; - moment(1) = 0.5f*c_ele*sqrtf(vel_body*vel_body)*vel_body(0)*_actuators.control[actuator_controls_s::INDEX_PITCH]; - moment(2) = 0.5f*c_rud*sqrtf(vel_body*vel_body)*vel_body(0)*_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); + moment(0) = c_ail*q*_actuators.control[actuator_controls_s::INDEX_ROLL] - c_damping*q*_omega(0); + moment(1) = c_ele*q*_actuators.control[actuator_controls_s::INDEX_PITCH]; + moment(2) = c_rud*q*_actuators.control[actuator_controls_s::INDEX_YAW]; + // introduce artificial time delay that is also present in acceleration + Vector3f moment_filtered; + moment_filtered(0) = _lp_filter_delay[0].apply(moment(0)); + moment_filtered(1) = _lp_filter_delay[1].apply(moment(1)); + moment_filtered(2) = _lp_filter_delay[2].apply(moment(2)); + // No filter for alpha, since it is already filtered... + // + Vector3f alpha_filtered = _alpha; Vector3f moment_command = _inertia * (ctrl - alpha_filtered) + moment_filtered; // perform dynamic inversion Vector3f deflection; - deflection(0) = moment_command(0)/(0.5f*c_ail*sqrtf(vel_body*vel_body)*vel_body(0)); - deflection(1) = moment_command(1)/(0.5f*c_ele*sqrtf(vel_body*vel_body)*vel_body(0)); - deflection(2) = moment_command(2)/(0.5f*c_rud*sqrtf(vel_body*vel_body)*vel_body(0)); + deflection(0) = (moment_command(0)+c_damping*q*_omega(0))/(c_ail*q); + deflection(1) = moment_command(1)/(c_ele*q); + deflection(2) = moment_command(2)/(c_rud*q); //PX4_INFO("filtered alpha: \t%.2f\t%.2f", (double)(_l_list(0))(1), (double)(_l_lpf_list(0))(1)); - return 1.f*deflection+ 0.f*(ctrl); + return deflection; } +/* Vector3f FixedwingPositionINDIControl::_apply_LP_filter(Vector3f new_input, Vector &old_input, Vector &old_output) { + // apply as: moment_filtered = _apply_LP_filter(moment, _m_list, _m_lpf_list); old_input(0) = old_input(1); old_input(1) = old_input(2); old_input(2) = new_input; @@ -958,6 +1000,7 @@ FixedwingPositionINDIControl::_apply_LP_filter(Vector3f new_input, Vector #include #include +#include #include #include #include @@ -118,6 +119,7 @@ private: uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; // vehicle attitude uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)}; // vehicle body accel uORB::Subscription _soaring_controller_status_sub{ORB_ID(soaring_controller_status)}; // vehicle status flags + uORB::Subscription _actuator_controls_sub{ORB_ID(actuator_controls_0)}; // Publishers uORB::Publication _actuators_0_pub; @@ -215,6 +217,7 @@ private: void vehicle_attitude_poll(); void vehicle_angular_velocity_poll(); void vehicle_angular_acceleration_poll(); + void actuator_controls_poll(); void control_update(); void manual_control_setpoint_poll(); @@ -281,6 +284,18 @@ private: Vector _w_lpf_list; // body rates Vector _a_lpf_list; Vector _l_lpf_list; + // controller frequency + const float _sample_frequency = 250.f; + // Low-Pass filters stage 1 + const float _cutoff_frequency_1 = 2.f; + math::LowPassFilter2p _lp_filter_accel[3] {{_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}}; // linear acceleration + math::LowPassFilter2p _lp_filter_force[3] {{_sample_frequency, 2}, {_sample_frequency, 2}, {_sample_frequency, 2}}; // force command + math::LowPassFilter2p _lp_filter_omega[3] {{_sample_frequency, 2}, {_sample_frequency, 2}, {_sample_frequency, 2}}; // body rates + // Low-Pass filters stage 2 + const float _cutoff_frequency_2 = 15.f; + math::LowPassFilter2p _lp_filter_delay[3] {{_sample_frequency, _cutoff_frequency_2}, {_sample_frequency, _cutoff_frequency_2}, {_sample_frequency, _cutoff_frequency_2}}; // filter to match acceleration processing delay + uint _counter = 0; + hrt_abstime _last_time{0}; // parameter variables Matrix3f _inertia {}; 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 0e02e52eb2..37e253eb99 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 @@ -189,8 +189,8 @@ PARAM_DEFINE_FLOAT(AOA_OFFSET, 0.01f); * @unit * @min -100 * @max 100 - * @decimal 4 - * @increment 0.0001 + * @decimal 7 + * @increment 0.0000001 * @group FW DYN SOAR Control */ PARAM_DEFINE_FLOAT(FILTER_A1, 1.16826067f); @@ -201,8 +201,8 @@ PARAM_DEFINE_FLOAT(FILTER_A1, 1.16826067f); * @unit * @min -100 * @max 100 - * @decimal 4 - * @increment 0.0001 + * @decimal 7 + * @increment 0.0000001 * @group FW DYN SOAR Control */ PARAM_DEFINE_FLOAT(FILTER_A2, -0.42411821f); @@ -213,8 +213,8 @@ PARAM_DEFINE_FLOAT(FILTER_A2, -0.42411821f); * @unit * @min -100 * @max 100 - * @decimal 4 - * @increment 0.0001 + * @decimal 7 + * @increment 0.0000001 * @group FW DYN SOAR Control */ PARAM_DEFINE_FLOAT(FILTER_B1, 0.06396438f); @@ -225,8 +225,8 @@ PARAM_DEFINE_FLOAT(FILTER_B1, 0.06396438f); * @unit * @min -100 * @max 100 - * @decimal 4 - * @increment 0.0001 + * @decimal 7 + * @increment 0.0000001 * @group FW DYN SOAR Control */ PARAM_DEFINE_FLOAT(FILTER_B2, 0.12792877f); @@ -237,8 +237,8 @@ PARAM_DEFINE_FLOAT(FILTER_B2, 0.12792877f); * @unit * @min -100 * @max 100 - * @decimal 4 - * @increment 0.0001 + * @decimal 7 + * @increment 0.0000001 * @group FW DYN SOAR Control */ PARAM_DEFINE_FLOAT(FILTER_B3, 0.06396438f); @@ -292,7 +292,7 @@ PARAM_DEFINE_FLOAT(K_X_YAW, 1.0f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(K_V_ROLL, 1.0f); +PARAM_DEFINE_FLOAT(K_V_ROLL, 1.3f); /** * pitch gain of K_v (velocity error gain) @@ -304,7 +304,7 @@ PARAM_DEFINE_FLOAT(K_V_ROLL, 1.0f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(K_V_PITCH, 1.0f); +PARAM_DEFINE_FLOAT(K_V_PITCH, 1.3f); /** * yaw gain of K_v (velocity error gain) @@ -316,7 +316,7 @@ PARAM_DEFINE_FLOAT(K_V_PITCH, 1.0f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(K_V_YAW, 1.0f); +PARAM_DEFINE_FLOAT(K_V_YAW, 1.3f); /** * roll gain of K_A (acceleration error gain) @@ -388,7 +388,7 @@ PARAM_DEFINE_FLOAT(K_Q_PITCH, 10.0f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(K_Q_YAW, 5.0f); +PARAM_DEFINE_FLOAT(K_Q_YAW, 2.0f); /** * roll gain of K_W (angular velocity error gain) @@ -424,7 +424,7 @@ PARAM_DEFINE_FLOAT(K_W_PITCH, 5.0f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(K_W_YAW, 5.0f); +PARAM_DEFINE_FLOAT(K_W_YAW, 1.0f); /** * roll gain of K_ACT (actuator deflection gain) @@ -432,11 +432,11 @@ PARAM_DEFINE_FLOAT(K_W_YAW, 5.0f); * @unit * @min 0 * @max 100 - * @decimal 1 - * @increment 0.1 + * @decimal 2 + * @increment 0.01 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(K_ACT_ROLL, 0.5f); +PARAM_DEFINE_FLOAT(K_ACT_ROLL, 0.3f); /** * pitch gain of K_ACT (actuator deflection gain) @@ -444,11 +444,11 @@ PARAM_DEFINE_FLOAT(K_ACT_ROLL, 0.5f); * @unit * @min 0 * @max 100 - * @decimal 1 - * @increment 0.1 + * @decimal 2 + * @increment 0.01 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(K_ACT_PITCH, 0.1f); +PARAM_DEFINE_FLOAT(K_ACT_PITCH, 0.2f); /** * yaw gain of K_ACT (actuator deflection gain) @@ -456,11 +456,11 @@ PARAM_DEFINE_FLOAT(K_ACT_PITCH, 0.1f); * @unit * @min 0 * @max 100 - * @decimal 1 - * @increment 0.1 + * @decimal 2 + * @increment 0.01 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(K_ACT_YAW, 0.1f); +PARAM_DEFINE_FLOAT(K_ACT_YAW, 1.f); // =================================================== // ============== trajectory center ================= diff --git a/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c b/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c index 202e89defa..a08e0de178 100644 --- a/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c +++ b/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c @@ -121,7 +121,7 @@ PARAM_DEFINE_INT32(IMU_GYRO_RATEMAX, 400); * @reboot_required true * @group Sensors */ -PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 20.0f); +PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 15.0f); /** * IMU gyro dynamic notch filtering