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

backup changes

Browse Source
This commit is contained in:
Marvin Harms
2022-06-06 09:33:37 +02:00
parent 93b2cd1cd8
commit 55992cd3c7
4 changed files with 143 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
+83 -13
View File
@@ -333,14 +333,21 @@ FixedwingPositionINDIControl::_set_wind_estimate(Vector3f wind)
return;
}
void
FixedwingPositionINDIControl::_select_trajectory(float initial_energy)
{
}
void
FixedwingPositionINDIControl::_read_trajectory_coeffs_csv(string filename)
{
// File pointer
std::ifstream fin;
// Open an existing file
// Open an existing file for testing
fin.open("/home/marvin/Documents/master_thesis_ADS/PX4/Git/ethzasl_fw_px4/src/modules/fw_dyn_soar_control/trajectories/"+filename, ios::in);
// Open an existing file from SD card
//fin.open("/fs/microsd/trajectories/"+filename);
// Read the Data from the file
@@ -821,6 +828,49 @@ FixedwingPositionINDIControl::_get_closest_t(Vector3f pos)
//PX4_INFO("closest point: \t%.2f\t%.2f\t%.2f", (double)_get_position_ref(t)(0), (double)_get_position_ref(t)(1), (double)_get_position_ref(t)(2));
//PX4_INFO("closest t: %.2f", (double)t);
//PX4_INFO("closest distance:%.2f", (double)sqrtf(min_dist));
/*
const uint n_multistage = 10;
Vector<float, n_multistage+1> distances_multistage;
float t_multistage = 0;
float t_ref_next = 0; // stage 2
// compute all distances
for(uint stage=1; stage<=2; stage++){
for(uint i=0; i<=n_multistage; i++){
t_ref = t_ref_next + float(i-n_multistage/2.f)/(powf(float(n_multistage),stage));
// check that t_ref is always in [0,1]:
if(t_ref<0.f){
t_ref += 1.f;
}
else if (t_ref>1.f){
t_ref -= 1.f;
}
Vector3f pos_ref = _get_position_ref(t_ref);
//PX4_INFO("trajectory time + point: \t%.2f\t%.2f\t%.2f\t%.2f", (double)t_ref, (double)pos_ref(0), (double)pos_ref(1), (double)pos_ref(2));
distances_multistage(i) = (pos_ref - pos)*(pos_ref - pos);
}
// get index of smallest distance
float min_dist_multistage = distances_multistage(0);
for(uint i=1; i<=n_multistage; i++){
if(distances_multistage(i)<min_dist_multistage){
min_dist_multistage = distances_multistage(i);
t_multistage = t_ref_next + float(i-n_multistage/2.f)/(powf(float(n_multistage),stage));
// check that t_ref is always in [0,1]:
if(t_multistage<0.f){
t_multistage += 1.f;
}
else if (t_multistage>1.f){
t_multistage -= 1.f;
}
}
}
// next starting point is previous closest point
t_ref_next = t_multistage;
}
PX4_INFO("different t: \t%.3f\t%.3f\t%.3f", (double)t, (double)t_multistage, (double)(t-t_multistage));
*/
return t;
}
@@ -931,7 +981,21 @@ FixedwingPositionINDIControl::_compute_NDI_stage_1(Vector3f pos_ref, Vector3f ve
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};
// ==========================================
// input meant for tuning the INDI controller
// ==========================================
/*
if(hrt_absolute_time()%2000000>1000000){
rot_acc_command = Vector3f{2.0f,1.f,0.f};
//rot_acc_command = Vector3f{0.f,0.f,0.5f};
}
else{
rot_acc_command = Vector3f{-2.0f,-1.f,0.f};
//rot_acc_command = Vector3f{0.f,0.f,-0.5f};
}
*/
//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));
@@ -942,10 +1006,12 @@ Vector3f
FixedwingPositionINDIControl::_compute_NDI_stage_2(Vector3f ctrl)
{
// compute the expected actuator efficiencies
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;
float k_ail = _param_k_act_roll.get();
float k_ele = _param_k_act_pitch.get();
float k_rud = _param_k_act_yaw.get();
float k_d_roll = _param_k_damping_roll.get();
float k_d_pitch = _param_k_damping_pitch.get();
float k_d_yaw = _param_k_damping_yaw.get();
// compute velocity in body frame
Dcmf R_ib(_att);
Vector3f vel_body = R_ib.transpose()*_vel;
@@ -953,11 +1019,16 @@ FixedwingPositionINDIControl::_compute_NDI_stage_2(Vector3f ctrl)
//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));
// filter omega at the same rate as the moments
Vector3f omega_filtered;
omega_filtered(0) = _lp_filter_omega_2[0].apply(_omega(0));
omega_filtered(1) = _lp_filter_omega_2[1].apply(_omega(1));
omega_filtered(2) = _lp_filter_omega_2[2].apply(_omega(2));
// compute moments
Vector3f moment;
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];
moment(0) = k_ail*q*_actuators.control[actuator_controls_s::INDEX_ROLL] - k_d_roll*q*omega_filtered(0);
moment(1) = k_ele*q*_actuators.control[actuator_controls_s::INDEX_PITCH] - k_d_pitch*q*omega_filtered(1);
moment(2) = k_rud*q*_actuators.control[actuator_controls_s::INDEX_YAW] - k_d_yaw*q*omega_filtered(2);
// introduce artificial time delay that is also present in acceleration
Vector3f moment_filtered;
moment_filtered(0) = _lp_filter_delay[0].apply(moment(0));
@@ -969,10 +1040,9 @@ FixedwingPositionINDIControl::_compute_NDI_stage_2(Vector3f ctrl)
Vector3f moment_command = _inertia * (ctrl - alpha_filtered) + moment_filtered;
// perform dynamic inversion
Vector3f deflection;
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));
deflection(0) = (moment_command(0) + k_d_roll*q*omega_filtered(0))/(k_ail*q);
deflection(1) = (moment_command(1) + k_d_pitch*q*omega_filtered(1))/(k_ele*q);
deflection(2) = (moment_command(2) + k_d_yaw*q*omega_filtered(2))/(k_rud*q);
return deflection;
}
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp
+16 -5
View File
@@ -186,9 +186,13 @@ private:
(ParamFloat<px4::params::K_W_ROLL>) _param_k_w_roll,
(ParamFloat<px4::params::K_W_PITCH>) _param_k_w_pitch,
(ParamFloat<px4::params::K_W_YAW>) _param_k_w_yaw,
// low-level controller params
(ParamFloat<px4::params::K_ACT_ROLL>) _param_k_act_roll,
(ParamFloat<px4::params::K_ACT_PITCH>) _param_k_act_pitch,
(ParamFloat<px4::params::K_ACT_YAW>) _param_k_act_yaw,
(ParamFloat<px4::params::K_DAMPING_ROLL>) _param_k_damping_roll,
(ParamFloat<px4::params::K_DAMPING_PITCH>) _param_k_damping_pitch,
(ParamFloat<px4::params::K_DAMPING_YAW>) _param_k_damping_yaw,
// location params
(ParamFloat<px4::params::ORIGIN_LAT>) _param_origin_lat,
(ParamFloat<px4::params::ORIGIN_LON>) _param_origin_lon,
@@ -233,7 +237,7 @@ private:
const static size_t _num_basis_funs = 16; // number of basis functions used for the trajectory approximation
// controller methods
void _select_trajectory(float initial_energy); // select the correct trajectory based on available energy
void _read_trajectory_coeffs_csv(std::string filename); // read in the correct coefficients of the appropriate trajectory
void _set_wind_estimate(Vector3f wind);
float _get_closest_t(Vector3f pos); // get the normalized time, at which the reference path is closest to the current position
@@ -287,13 +291,14 @@ private:
// controller frequency
const float _sample_frequency = 250.f;
// Low-Pass filters stage 1
const float _cutoff_frequency_1 = 2.f;
const float _cutoff_frequency_1 = 5.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
math::LowPassFilter2p _lp_filter_force[3] {{_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}}; // force command
math::LowPassFilter2p _lp_filter_omega[3] {{_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}}; // body rates
// Low-Pass filters stage 2
const float _cutoff_frequency_2 = 15.f;
const float _cutoff_frequency_2 = 5.f; // MUST MATCH PARAM "IMU_DGYRO_CUTOFF"
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
math::LowPassFilter2p _lp_filter_omega_2[3] {{_sample_frequency, _cutoff_frequency_2}, {_sample_frequency, _cutoff_frequency_2}, {_sample_frequency, _cutoff_frequency_2}}; // body rates
uint _counter = 0;
hrt_abstime _last_time{0};
@@ -334,6 +339,12 @@ private:
hrt_abstime _slip_last_valid{0}; ///< last time Aoa was received. Used to detect timeouts.
float _slip{0.0f};
// vectors defining the initial velocities, wind speed and shear strength
Vector<float, 10> _initial_velocities_trajectory = {};
Vector<float, 10> _wind_speed_trajectory = {};
Vector<float, 10> _shear_param_trajectory = {};
// helper variables
Dcmf _R_ned_to_enu; // rotation matrix from NED to ENU frame
Dcmf _R_enu_to_ned; // rotation matrix from ENU to NED frame
src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c
+43 -3
View File
@@ -426,6 +426,10 @@ PARAM_DEFINE_FLOAT(K_W_PITCH, 5.0f);
*/
PARAM_DEFINE_FLOAT(K_W_YAW, 1.0f);
// =============================
// low level INDI control params
// =============================
/**
* roll gain of K_ACT (actuator deflection gain)
*
@@ -436,7 +440,7 @@ PARAM_DEFINE_FLOAT(K_W_YAW, 1.0f);
* @increment 0.01
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_FLOAT(K_ACT_ROLL, 0.3f);
PARAM_DEFINE_FLOAT(K_ACT_ROLL, 0.1f);
/**
* pitch gain of K_ACT (actuator deflection gain)
@@ -448,7 +452,7 @@ PARAM_DEFINE_FLOAT(K_ACT_ROLL, 0.3f);
* @increment 0.01
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_FLOAT(K_ACT_PITCH, 0.2f);
PARAM_DEFINE_FLOAT(K_ACT_PITCH, 0.03f);
/**
* yaw gain of K_ACT (actuator deflection gain)
@@ -460,7 +464,43 @@ PARAM_DEFINE_FLOAT(K_ACT_PITCH, 0.2f);
* @increment 0.01
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_FLOAT(K_ACT_YAW, 1.f);
PARAM_DEFINE_FLOAT(K_ACT_YAW, 0.02f);
/**
* roll gain of K_ACT (actuator deflection gain)
*
* @unit
* @min 0
* @max 100
* @decimal 2
* @increment 0.01
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_FLOAT(K_DAMPING_ROLL, 0.02f);
/**
* pitch gain of K_ACT (actuator deflection gain)
*
* @unit
* @min 0
* @max 100
* @decimal 2
* @increment 0.01
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_FLOAT(K_DAMPING_PITCH, 0.01f);
/**
* yaw gain of K_ACT (actuator deflection gain)
*
* @unit
* @min 0
* @max 100
* @decimal 2
* @increment 0.01
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_FLOAT(K_DAMPING_YAW, 0.0f);
// ===================================================
// ============== 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, 15.0f);
PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 5.0f);
/**
* IMU gyro dynamic notch filtering