fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-13 14:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

changed incremental filter settings

Browse Source
This commit is contained in:
Marvin Harms
2022-05-05 14:53:50 +02:00
parent 2886705d06
commit 93b2cd1cd8
4 changed files with 103 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
+63 -20
View File
@@ -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<Vector3f, 3> &old_input, Vector<Vector3f, 2> &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<Vector
//
return output;
}
*/
Vector3f
FixedwingPositionINDIControl::_compute_actuator_deflections(Vector3f ctrl)
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp
+15
View File
@@ -25,6 +25,7 @@
#include <lib/l1/ECL_L1_Pos_Controller.hpp>
#include <lib/landing_slope/Landingslope.hpp>
#include <lib/mathlib/mathlib.h>
#include <lib/mathlib/math/filter/LowPassFilter2p.hpp>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
@@ -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<actuator_controls_s> _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<Vector3f, 2> _w_lpf_list; // body rates
Vector<Vector3f, 2> _a_lpf_list;
Vector<Vector3f, 2> _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 {};
src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c
+24 -24
View File
@@ -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 =================
src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c
+1 -1
View File
@@ -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