mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-08-21 14:40:36 +08:00
renamed params, set freqto 100, rm SITL lockstep
This commit is contained in:
Marvin Harms
@@ -119,5 +119,5 @@ if(REPLAY_FILE)
|
||||
message(STATUS "Building without lockstep for replay")
|
||||
set(ENABLE_LOCKSTEP_SCHEDULER no)
|
||||
else()
|
||||
set(ENABLE_LOCKSTEP_SCHEDULER yes)
|
||||
set(ENABLE_LOCKSTEP_SCHEDULER no)
|
||||
endif()
|
||||
|
||||
@@ -57,8 +57,9 @@ FixedwingPositionINDIControl::FixedwingPositionINDIControl() :
|
||||
_attitude_sp_pub(ORB_ID(vehicle_attitude_setpoint)),
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
|
||||
{
|
||||
// limit to 50 Hz
|
||||
//_vehicle_angular_velocity_sub.set_interval_ms(20);
|
||||
// limit to 100 Hz
|
||||
_vehicle_angular_velocity_sub.set_interval_ms(1000/_sample_frequency);
|
||||
|
||||
|
||||
/* fetch initial parameter values */
|
||||
parameters_update();
|
||||
@@ -381,68 +382,12 @@ FixedwingPositionINDIControl::_select_trajectory(float initial_energy)
|
||||
void
|
||||
FixedwingPositionINDIControl::_read_trajectory_coeffs_csv(char *filename)
|
||||
{
|
||||
/*
|
||||
// File pointer
|
||||
std::ifstream fin;
|
||||
|
||||
// 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(PX4_ROOTFSDIR"/fs/microsd/trajectories/"+filename);
|
||||
|
||||
// Read the Data from the file
|
||||
// as String Vector
|
||||
string line, word;
|
||||
|
||||
bool error = false;
|
||||
|
||||
// loop over x, y, z
|
||||
for (int l=0; l<3; l++) {
|
||||
getline(fin, line, '\n');
|
||||
|
||||
// used for breaking words
|
||||
stringstream s(line);
|
||||
|
||||
// read every column data of a row and
|
||||
// store it in a string variable, 'word'
|
||||
uint i = 0;
|
||||
while (getline(s, word, ',')) {
|
||||
// catch error
|
||||
if(i>=_num_basis_funs){
|
||||
PX4_ERR("number of coefficients is too large.");
|
||||
error = true;
|
||||
}
|
||||
else{
|
||||
switch(l){
|
||||
case 0:
|
||||
_basis_coeffs_x(i) = stof(word);
|
||||
break;
|
||||
case 1:
|
||||
_basis_coeffs_y(i) = stof(word);
|
||||
break;
|
||||
case 2:
|
||||
_basis_coeffs_z(i) = stof(word);
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
//PX4_INFO("coefficient value: %.6f", (double)stof(word));
|
||||
i += 1;
|
||||
}
|
||||
// catch error
|
||||
if(_num_basis_funs-i>1){
|
||||
PX4_ERR("number of coefficients is too small: %.f", (double)(i));
|
||||
error = true;
|
||||
}
|
||||
}
|
||||
fin.close();
|
||||
*/
|
||||
|
||||
// =======================================================================
|
||||
bool error = false;
|
||||
|
||||
//char home_dir[200] = "/home/marvin/Documents/master_thesis_ADS/PX4/Git/ethzasl_fw_px4/src/modules/fw_dyn_soar_control/trajectories/";
|
||||
char home_dir[200] = PX4_ROOTFSDIR"/fs/microsd/trajectories/";
|
||||
char home_dir[200] = "/home/marvin/Documents/master_thesis_ADS/PX4/Git/ethzasl_fw_px4/src/modules/fw_dyn_soar_control/trajectories/";
|
||||
//char home_dir[200] = PX4_ROOTFSDIR"/fs/microsd/trajectories/";
|
||||
//PX4_ERR(home_dir);
|
||||
strcat(home_dir,filename);
|
||||
FILE* fp = fopen(home_dir, "r");
|
||||
@@ -485,7 +430,7 @@ FixedwingPositionINDIControl::_read_trajectory_coeffs_csv(char *filename)
|
||||
default:
|
||||
break;
|
||||
}
|
||||
PX4_INFO("row: %d, col: %d, read value: %.3f", row, column, (double)atof(value));
|
||||
//PX4_INFO("row: %d, col: %d, read value: %.3f", row, column, (double)atof(value));
|
||||
value = strtok(NULL, ",");
|
||||
column++;
|
||||
|
||||
@@ -695,7 +640,6 @@ FixedwingPositionINDIControl::Run()
|
||||
_angular_accel_sp.xyz[1] = ctrl(1);
|
||||
_angular_accel_sp.xyz[2] = ctrl(2);
|
||||
_angular_accel_sp_pub.publish(_angular_accel_sp);
|
||||
//print_message(_angular_accel_sp);
|
||||
|
||||
// =========================
|
||||
// publish attitude setpoint
|
||||
@@ -1131,7 +1075,7 @@ FixedwingPositionINDIControl::_compute_NDI_stage_1(Vector3f pos_ref, Vector3f ve
|
||||
// ==========================================
|
||||
// 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};
|
||||
@@ -1140,7 +1084,7 @@ FixedwingPositionINDIControl::_compute_NDI_stage_1(Vector3f pos_ref, Vector3f ve
|
||||
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", (double)(f_command*f_command));
|
||||
//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));
|
||||
|
||||
@@ -156,56 +156,56 @@ private:
|
||||
// parameter struct
|
||||
DEFINE_PARAMETERS(
|
||||
// aircraft params
|
||||
(ParamFloat<px4::params::FW_INERTIA_ROLL>) _param_fw_inertia_roll,
|
||||
(ParamFloat<px4::params::FW_INERTIA_PITCH>) _param_fw_inertia_pitch,
|
||||
(ParamFloat<px4::params::FW_INERTIA_YAW>) _param_fw_inertia_yaw,
|
||||
(ParamFloat<px4::params::FW_MASS>) _param_fw_mass,
|
||||
(ParamFloat<px4::params::FW_WING_AREA>) _param_fw_wing_area,
|
||||
(ParamFloat<px4::params::RHO>) _param_rho,
|
||||
(ParamFloat<px4::params::DS_INERTIA_ROLL>) _param_fw_inertia_roll,
|
||||
(ParamFloat<px4::params::DS_INERTIA_PITCH>) _param_fw_inertia_pitch,
|
||||
(ParamFloat<px4::params::DS_INERTIA_YAW>) _param_fw_inertia_yaw,
|
||||
(ParamFloat<px4::params::DS_MASS>) _param_fw_mass,
|
||||
(ParamFloat<px4::params::DS_WING_AREA>) _param_fw_wing_area,
|
||||
(ParamFloat<px4::params::DS_RHO>) _param_rho,
|
||||
// aerodynamic params
|
||||
(ParamFloat<px4::params::C_L0>) _param_fw_c_l0,
|
||||
(ParamFloat<px4::params::C_L1>) _param_fw_c_l1,
|
||||
(ParamFloat<px4::params::C_D0>) _param_fw_c_d0,
|
||||
(ParamFloat<px4::params::C_D1>) _param_fw_c_d1,
|
||||
(ParamFloat<px4::params::C_D2>) _param_fw_c_d2,
|
||||
(ParamFloat<px4::params::AOA_OFFSET>) _param_aoa_offset,
|
||||
(ParamFloat<px4::params::DS_C_L0>) _param_fw_c_l0,
|
||||
(ParamFloat<px4::params::DS_C_L1>) _param_fw_c_l1,
|
||||
(ParamFloat<px4::params::DS_C_D0>) _param_fw_c_d0,
|
||||
(ParamFloat<px4::params::DS_C_D1>) _param_fw_c_d1,
|
||||
(ParamFloat<px4::params::DS_C_D2>) _param_fw_c_d2,
|
||||
(ParamFloat<px4::params::DS_AOA_OFFSET>) _param_aoa_offset,
|
||||
// filter params
|
||||
(ParamFloat<px4::params::FILTER_A1>) _param_filter_a1,
|
||||
(ParamFloat<px4::params::FILTER_A2>) _param_filter_a2,
|
||||
(ParamFloat<px4::params::FILTER_B1>) _param_filter_b1,
|
||||
(ParamFloat<px4::params::FILTER_B2>) _param_filter_b2,
|
||||
(ParamFloat<px4::params::FILTER_B3>) _param_filter_b3,
|
||||
(ParamFloat<px4::params::DS_FILTER_A1>) _param_filter_a1,
|
||||
(ParamFloat<px4::params::DS_FILTER_A2>) _param_filter_a2,
|
||||
(ParamFloat<px4::params::DS_FILTER_B1>) _param_filter_b1,
|
||||
(ParamFloat<px4::params::DS_FILTER_B2>) _param_filter_b2,
|
||||
(ParamFloat<px4::params::DS_FILTER_B3>) _param_filter_b3,
|
||||
// controller params
|
||||
(ParamFloat<px4::params::K_X_ROLL>) _param_k_x_roll,
|
||||
(ParamFloat<px4::params::K_X_PITCH>) _param_k_x_pitch,
|
||||
(ParamFloat<px4::params::K_X_YAW>) _param_k_x_yaw,
|
||||
(ParamFloat<px4::params::K_V_ROLL>) _param_k_v_roll,
|
||||
(ParamFloat<px4::params::K_V_PITCH>) _param_k_v_pitch,
|
||||
(ParamFloat<px4::params::K_V_YAW>) _param_k_v_yaw,
|
||||
(ParamFloat<px4::params::K_A_ROLL>) _param_k_a_roll,
|
||||
(ParamFloat<px4::params::K_A_PITCH>) _param_k_a_pitch,
|
||||
(ParamFloat<px4::params::K_A_YAW>) _param_k_a_yaw,
|
||||
(ParamFloat<px4::params::K_Q_ROLL>) _param_k_q_roll,
|
||||
(ParamFloat<px4::params::K_Q_PITCH>) _param_k_q_pitch,
|
||||
(ParamFloat<px4::params::K_Q_YAW>) _param_k_q_yaw,
|
||||
(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,
|
||||
(ParamFloat<px4::params::DS_K_X_ROLL>) _param_k_x_roll,
|
||||
(ParamFloat<px4::params::DS_K_X_PITCH>) _param_k_x_pitch,
|
||||
(ParamFloat<px4::params::DS_K_X_YAW>) _param_k_x_yaw,
|
||||
(ParamFloat<px4::params::DS_K_V_ROLL>) _param_k_v_roll,
|
||||
(ParamFloat<px4::params::DS_K_V_PITCH>) _param_k_v_pitch,
|
||||
(ParamFloat<px4::params::DS_K_V_YAW>) _param_k_v_yaw,
|
||||
(ParamFloat<px4::params::DS_K_A_ROLL>) _param_k_a_roll,
|
||||
(ParamFloat<px4::params::DS_K_A_PITCH>) _param_k_a_pitch,
|
||||
(ParamFloat<px4::params::DS_K_A_YAW>) _param_k_a_yaw,
|
||||
(ParamFloat<px4::params::DS_K_Q_ROLL>) _param_k_q_roll,
|
||||
(ParamFloat<px4::params::DS_K_Q_PITCH>) _param_k_q_pitch,
|
||||
(ParamFloat<px4::params::DS_K_Q_YAW>) _param_k_q_yaw,
|
||||
(ParamFloat<px4::params::DS_K_W_ROLL>) _param_k_w_roll,
|
||||
(ParamFloat<px4::params::DS_K_W_PITCH>) _param_k_w_pitch,
|
||||
(ParamFloat<px4::params::DS_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,
|
||||
(ParamFloat<px4::params::DS_K_ACT_ROLL>) _param_k_act_roll,
|
||||
(ParamFloat<px4::params::DS_K_ACT_PITCH>) _param_k_act_pitch,
|
||||
(ParamFloat<px4::params::DS_K_ACT_YAW>) _param_k_act_yaw,
|
||||
(ParamFloat<px4::params::DS_K_DAMP_ROLL>) _param_k_damping_roll,
|
||||
(ParamFloat<px4::params::DS_K_DAMP_PITCH>) _param_k_damping_pitch,
|
||||
(ParamFloat<px4::params::DS_K_DAMP_YAW>) _param_k_damping_yaw,
|
||||
// location params
|
||||
(ParamFloat<px4::params::ORIGIN_LAT>) _param_origin_lat,
|
||||
(ParamFloat<px4::params::ORIGIN_LON>) _param_origin_lon,
|
||||
(ParamFloat<px4::params::ORIGIN_ALT>) _param_origin_alt,
|
||||
(ParamFloat<px4::params::DS_ORIGIN_LAT>) _param_origin_lat,
|
||||
(ParamFloat<px4::params::DS_ORIGIN_LON>) _param_origin_lon,
|
||||
(ParamFloat<px4::params::DS_ORIGIN_ALT>) _param_origin_alt,
|
||||
// loiter params
|
||||
(ParamInt<px4::params::LOITER>) _param_loiter,
|
||||
(ParamInt<px4::params::DS_LOITER>) _param_loiter,
|
||||
// thrust params
|
||||
(ParamFloat<px4::params::THRUST>) _param_thrust
|
||||
(ParamFloat<px4::params::DS_THRUST>) _param_thrust
|
||||
|
||||
)
|
||||
|
||||
@@ -298,7 +298,7 @@ private:
|
||||
Vector<Vector3f, 2> _a_lpf_list;
|
||||
Vector<Vector3f, 2> _l_lpf_list;
|
||||
// controller frequency
|
||||
const float _sample_frequency = 250.f;
|
||||
const float _sample_frequency = 100.f;
|
||||
// Low-Pass filters stage 1
|
||||
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
|
||||
|
||||
@@ -22,7 +22,7 @@
|
||||
* @increment 0.01
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_INERTIA_ROLL, 0.197563f);
|
||||
PARAM_DEFINE_FLOAT(DS_INERTIA_ROLL, 0.197563f);
|
||||
|
||||
/**
|
||||
* inertia around body y-axis
|
||||
@@ -36,7 +36,7 @@ PARAM_DEFINE_FLOAT(FW_INERTIA_ROLL, 0.197563f);
|
||||
* @increment 0.01
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_INERTIA_PITCH, 0.1458929f);
|
||||
PARAM_DEFINE_FLOAT(DS_INERTIA_PITCH, 0.1458929f);
|
||||
|
||||
/**
|
||||
* inertia around body z-axis
|
||||
@@ -50,7 +50,7 @@ PARAM_DEFINE_FLOAT(FW_INERTIA_PITCH, 0.1458929f);
|
||||
* @increment 0.01
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_INERTIA_YAW, 0.1477f);
|
||||
PARAM_DEFINE_FLOAT(DS_INERTIA_YAW, 0.1477f);
|
||||
|
||||
|
||||
|
||||
@@ -66,7 +66,7 @@ PARAM_DEFINE_FLOAT(FW_INERTIA_YAW, 0.1477f);
|
||||
* @increment 0.01
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_MASS, 1.4f);
|
||||
PARAM_DEFINE_FLOAT(DS_MASS, 1.4f);
|
||||
|
||||
/**
|
||||
* total wing area used for lift and drag computation
|
||||
@@ -78,7 +78,7 @@ PARAM_DEFINE_FLOAT(FW_MASS, 1.4f);
|
||||
* @increment 0.01
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_WING_AREA, 0.4f);
|
||||
PARAM_DEFINE_FLOAT(DS_WING_AREA, 0.4f);
|
||||
|
||||
|
||||
/**
|
||||
@@ -91,7 +91,7 @@ PARAM_DEFINE_FLOAT(FW_WING_AREA, 0.4f);
|
||||
* @increment 0.001
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(RHO, 1.223f);
|
||||
PARAM_DEFINE_FLOAT(DS_RHO, 1.223f);
|
||||
|
||||
/**
|
||||
* estimated lift coefficients used for lift and drag computation
|
||||
@@ -106,7 +106,7 @@ PARAM_DEFINE_FLOAT(RHO, 1.223f);
|
||||
* @increment 0.001
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(C_L0, 0.356f);
|
||||
PARAM_DEFINE_FLOAT(DS_C_L0, 0.356f);
|
||||
|
||||
/**
|
||||
* estimated lift coefficients used for lift and drag computation
|
||||
@@ -121,7 +121,7 @@ PARAM_DEFINE_FLOAT(C_L0, 0.356f);
|
||||
* @increment 0.001
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(C_L1, 2.354f);
|
||||
PARAM_DEFINE_FLOAT(DS_C_L1, 2.354f);
|
||||
|
||||
|
||||
/**
|
||||
@@ -137,7 +137,7 @@ PARAM_DEFINE_FLOAT(C_L1, 2.354f);
|
||||
* @increment 0.0001
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(C_D0, 0.0288f);
|
||||
PARAM_DEFINE_FLOAT(DS_C_D0, 0.0288f);
|
||||
|
||||
/**
|
||||
* estimated drag coefficients used for lift and drag computation
|
||||
@@ -152,7 +152,7 @@ PARAM_DEFINE_FLOAT(C_D0, 0.0288f);
|
||||
* @increment 0.0001
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(C_D1, 0.3783f);
|
||||
PARAM_DEFINE_FLOAT(DS_C_D1, 0.3783f);
|
||||
|
||||
/**
|
||||
* estimated drag coefficients used for lift and drag computation
|
||||
@@ -167,7 +167,7 @@ PARAM_DEFINE_FLOAT(C_D1, 0.3783f);
|
||||
* @increment 0.0001
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(C_D2, 1.984f);
|
||||
PARAM_DEFINE_FLOAT(DS_C_D2, 1.984f);
|
||||
|
||||
/**
|
||||
* offset angle between body frame (Pixhawk) and the wing chord
|
||||
@@ -181,7 +181,7 @@ PARAM_DEFINE_FLOAT(C_D2, 1.984f);
|
||||
* @increment 0.01
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(AOA_OFFSET, 0.01f);
|
||||
PARAM_DEFINE_FLOAT(DS_AOA_OFFSET, 0.01f);
|
||||
|
||||
/**
|
||||
* coefficients of the butterworth filter used for smoothing the IMU
|
||||
@@ -193,7 +193,7 @@ PARAM_DEFINE_FLOAT(AOA_OFFSET, 0.01f);
|
||||
* @increment 0.0000001
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FILTER_A1, 1.16826067f);
|
||||
PARAM_DEFINE_FLOAT(DS_FILTER_A1, 1.16826067f);
|
||||
|
||||
/**
|
||||
* coefficients of the butterworth filter used for smoothing the IMU
|
||||
@@ -205,7 +205,7 @@ PARAM_DEFINE_FLOAT(FILTER_A1, 1.16826067f);
|
||||
* @increment 0.0000001
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FILTER_A2, -0.42411821f);
|
||||
PARAM_DEFINE_FLOAT(DS_FILTER_A2, -0.42411821f);
|
||||
|
||||
/**
|
||||
* coefficients of the butterworth filter used for smoothing the IMU
|
||||
@@ -217,7 +217,7 @@ PARAM_DEFINE_FLOAT(FILTER_A2, -0.42411821f);
|
||||
* @increment 0.0000001
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FILTER_B1, 0.06396438f);
|
||||
PARAM_DEFINE_FLOAT(DS_FILTER_B1, 0.06396438f);
|
||||
|
||||
/**
|
||||
* coefficients of the butterworth filter used for smoothing the IMU
|
||||
@@ -229,7 +229,7 @@ PARAM_DEFINE_FLOAT(FILTER_B1, 0.06396438f);
|
||||
* @increment 0.0000001
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FILTER_B2, 0.12792877f);
|
||||
PARAM_DEFINE_FLOAT(DS_FILTER_B2, 0.12792877f);
|
||||
|
||||
/**
|
||||
* coefficients of the butterworth filter used for smoothing the IMU
|
||||
@@ -241,7 +241,7 @@ PARAM_DEFINE_FLOAT(FILTER_B2, 0.12792877f);
|
||||
* @increment 0.0000001
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FILTER_B3, 0.06396438f);
|
||||
PARAM_DEFINE_FLOAT(DS_FILTER_B3, 0.06396438f);
|
||||
|
||||
// ========================================================
|
||||
// =================== CONTROL GAINS ======================
|
||||
@@ -256,7 +256,7 @@ PARAM_DEFINE_FLOAT(FILTER_B3, 0.06396438f);
|
||||
* @increment 0.1
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(K_X_ROLL, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(DS_K_X_ROLL, 1.0f);
|
||||
|
||||
/**
|
||||
* pitch gain of K_x (position error gain)
|
||||
@@ -268,7 +268,7 @@ PARAM_DEFINE_FLOAT(K_X_ROLL, 1.0f);
|
||||
* @increment 0.1
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(K_X_PITCH, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(DS_K_X_PITCH, 1.0f);
|
||||
|
||||
/**
|
||||
* yaw gain of K_x (position error gain)
|
||||
@@ -280,7 +280,7 @@ PARAM_DEFINE_FLOAT(K_X_PITCH, 1.0f);
|
||||
* @increment 0.1
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(K_X_YAW, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(DS_K_X_YAW, 1.0f);
|
||||
|
||||
/**
|
||||
* roll gain of K_v (velocity error gain)
|
||||
@@ -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.3f);
|
||||
PARAM_DEFINE_FLOAT(DS_K_V_ROLL, 1.3f);
|
||||
|
||||
/**
|
||||
* pitch gain of K_v (velocity error gain)
|
||||
@@ -304,7 +304,7 @@ PARAM_DEFINE_FLOAT(K_V_ROLL, 1.3f);
|
||||
* @increment 0.1
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(K_V_PITCH, 1.3f);
|
||||
PARAM_DEFINE_FLOAT(DS_K_V_PITCH, 1.3f);
|
||||
|
||||
/**
|
||||
* yaw gain of K_v (velocity error gain)
|
||||
@@ -316,7 +316,7 @@ PARAM_DEFINE_FLOAT(K_V_PITCH, 1.3f);
|
||||
* @increment 0.1
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(K_V_YAW, 1.3f);
|
||||
PARAM_DEFINE_FLOAT(DS_K_V_YAW, 1.3f);
|
||||
|
||||
/**
|
||||
* roll gain of K_A (acceleration error gain)
|
||||
@@ -328,7 +328,7 @@ PARAM_DEFINE_FLOAT(K_V_YAW, 1.3f);
|
||||
* @increment 0.1
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(K_A_ROLL, 0.5f);
|
||||
PARAM_DEFINE_FLOAT(DS_K_A_ROLL, 0.5f);
|
||||
|
||||
/**
|
||||
* pitch gain of K_A (acceleration error gain)
|
||||
@@ -340,7 +340,7 @@ PARAM_DEFINE_FLOAT(K_A_ROLL, 0.5f);
|
||||
* @increment 0.1
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(K_A_PITCH, 0.5f);
|
||||
PARAM_DEFINE_FLOAT(DS_K_A_PITCH, 0.5f);
|
||||
|
||||
/**
|
||||
* yaw gain of K_A (acceleration error gain)
|
||||
@@ -352,7 +352,7 @@ PARAM_DEFINE_FLOAT(K_A_PITCH, 0.5f);
|
||||
* @increment 0.1
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(K_A_YAW, 0.5f);
|
||||
PARAM_DEFINE_FLOAT(DS_K_A_YAW, 0.5f);
|
||||
|
||||
/**
|
||||
* roll gain of K_Q (attitude error gain)
|
||||
@@ -364,7 +364,7 @@ PARAM_DEFINE_FLOAT(K_A_YAW, 0.5f);
|
||||
* @increment 0.1
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(K_Q_ROLL, 10.0f);
|
||||
PARAM_DEFINE_FLOAT(DS_K_Q_ROLL, 10.0f);
|
||||
|
||||
/**
|
||||
* pitch gain of K_Q (attitude error gain)
|
||||
@@ -376,7 +376,7 @@ PARAM_DEFINE_FLOAT(K_Q_ROLL, 10.0f);
|
||||
* @increment 0.1
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(K_Q_PITCH, 10.0f);
|
||||
PARAM_DEFINE_FLOAT(DS_K_Q_PITCH, 10.0f);
|
||||
|
||||
/**
|
||||
* yaw gain of K_Q (attitude 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, 2.0f);
|
||||
PARAM_DEFINE_FLOAT(DS_K_Q_YAW, 2.0f);
|
||||
|
||||
/**
|
||||
* roll gain of K_W (angular velocity error gain)
|
||||
@@ -400,7 +400,7 @@ PARAM_DEFINE_FLOAT(K_Q_YAW, 2.0f);
|
||||
* @increment 0.1
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(K_W_ROLL, 5.0f);
|
||||
PARAM_DEFINE_FLOAT(DS_K_W_ROLL, 5.0f);
|
||||
|
||||
/**
|
||||
* pitch gain of K_W (angular velocity error gain)
|
||||
@@ -412,7 +412,7 @@ PARAM_DEFINE_FLOAT(K_W_ROLL, 5.0f);
|
||||
* @increment 0.1
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(K_W_PITCH, 5.0f);
|
||||
PARAM_DEFINE_FLOAT(DS_K_W_PITCH, 5.0f);
|
||||
|
||||
/**
|
||||
* yaw 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, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(DS_K_W_YAW, 1.0f);
|
||||
|
||||
// =============================
|
||||
// low level INDI control params
|
||||
@@ -440,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.1f);
|
||||
PARAM_DEFINE_FLOAT(DS_K_ACT_ROLL, 0.1f);
|
||||
|
||||
/**
|
||||
* pitch gain of K_ACT (actuator deflection gain)
|
||||
@@ -452,7 +452,7 @@ PARAM_DEFINE_FLOAT(K_ACT_ROLL, 0.1f);
|
||||
* @increment 0.01
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(K_ACT_PITCH, 0.03f);
|
||||
PARAM_DEFINE_FLOAT(DS_K_ACT_PITCH, 0.03f);
|
||||
|
||||
/**
|
||||
* yaw gain of K_ACT (actuator deflection gain)
|
||||
@@ -464,7 +464,7 @@ PARAM_DEFINE_FLOAT(K_ACT_PITCH, 0.03f);
|
||||
* @increment 0.01
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(K_ACT_YAW, 0.1f);
|
||||
PARAM_DEFINE_FLOAT(DS_K_ACT_YAW, 0.1f);
|
||||
|
||||
/**
|
||||
* roll gain of K_ACT_DAMPING (actuator damping gain)
|
||||
@@ -476,7 +476,7 @@ PARAM_DEFINE_FLOAT(K_ACT_YAW, 0.1f);
|
||||
* @increment 0.01
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(K_DAMPING_ROLL, 0.02f);
|
||||
PARAM_DEFINE_FLOAT(DS_K_DAMP_ROLL, 0.02f);
|
||||
|
||||
/**
|
||||
* pitch gain of K_ACT_DAMPING (actuator damping gain)
|
||||
@@ -488,7 +488,7 @@ PARAM_DEFINE_FLOAT(K_DAMPING_ROLL, 0.02f);
|
||||
* @increment 0.01
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(K_DAMPING_PITCH, 0.01f);
|
||||
PARAM_DEFINE_FLOAT(DS_K_DAMP_PITCH, 0.01f);
|
||||
|
||||
/**
|
||||
* yaw gain of K_ACT_DAMPING (actuator damping gain)
|
||||
@@ -500,7 +500,7 @@ PARAM_DEFINE_FLOAT(K_DAMPING_PITCH, 0.01f);
|
||||
* @increment 0.01
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(K_DAMPING_YAW, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(DS_K_DAMP_YAW, 0.0f);
|
||||
|
||||
// ===================================================
|
||||
// ============== trajectory center =================
|
||||
@@ -516,7 +516,7 @@ PARAM_DEFINE_FLOAT(K_DAMPING_YAW, 0.0f);
|
||||
* @increment 0.0000001
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(ORIGIN_LAT, 47.39797f);
|
||||
PARAM_DEFINE_FLOAT(DS_ORIGIN_LAT, 47.39797f);
|
||||
|
||||
/**
|
||||
* longitude of trajectory start point (WGS84)
|
||||
@@ -528,7 +528,7 @@ PARAM_DEFINE_FLOAT(ORIGIN_LAT, 47.39797f);
|
||||
* @increment 0.0000001
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(ORIGIN_LON, 8.54554f);
|
||||
PARAM_DEFINE_FLOAT(DS_ORIGIN_LON, 8.54554f);
|
||||
|
||||
/**
|
||||
* altitude of trajectory start point (WGS84)
|
||||
@@ -540,7 +540,7 @@ PARAM_DEFINE_FLOAT(ORIGIN_LON, 8.54554f);
|
||||
* @increment 0.1
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(ORIGIN_ALT, 488.0f);
|
||||
PARAM_DEFINE_FLOAT(DS_ORIGIN_ALT, 488.0f);
|
||||
|
||||
|
||||
// ======================================================
|
||||
@@ -557,7 +557,7 @@ PARAM_DEFINE_FLOAT(ORIGIN_ALT, 488.0f);
|
||||
* @increment 1
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_INT32(LOITER, 0);
|
||||
PARAM_DEFINE_INT32(DS_LOITER, 0);
|
||||
|
||||
// ==============================================
|
||||
// ============== engine thrust =================
|
||||
@@ -572,4 +572,4 @@ PARAM_DEFINE_INT32(LOITER, 0);
|
||||
* @increment 0.1
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(THRUST, 0);
|
||||
PARAM_DEFINE_FLOAT(DS_THRUST, 0);
|
||||
@@ -1,3 +1,3 @@
|
||||
-0.000014,-3125.684664,11089.049565,-18942.770235,16837.269583,-2890.234017,-11837.475189,13153.935749,-141.459060,-12990.951555,11934.676728,2594.466297,-16521.457469,18740.250392,-11010.786326,3111.272558
|
||||
-99.995573,-5095.160841,24136.272616,-56105.353355,80560.078564,-70163.335913,19749.848200,41159.562505,-68048.750290,40306.617839,21058.585202,-71404.243825,81395.788686,-56504.078947,24259.929878,-5114.204474
|
||||
100.000000,-0.006882,-0.009535,-0.081985,-0.006197,-0.086599,0.025787,0.106096,-0.069921,0.008318,0.030489,-0.376848,0.061624,-0.128956,0.060856,-0.002952
|
||||
-0.000,-3125.685,11089.050,-18942.770,16837.270,-2890.234,-11837.475,13153.936,-141.459,-12990.952,11934.677,2594.466,-16521.457,18740.250,-11010.786,3111.273
|
||||
-99.996,-5095.161,24136.273,-56105.353,80560.079,-70163.336,19749.848,41159.563,-68048.750,40306.618,21058.585,-71404.244,81395.789,-56504.079,24259.930,-5114.204
|
||||
100.000,-0.007,-0.010,-0.082,-0.006,-0.087,0.026,0.106,-0.070,0.008,0.030,-0.377,0.062,-0.129,0.061,-0.003
|
||||
|
||||
|
@@ -1,3 +1,3 @@
|
||||
-0.000008,-1875.410798,6653.429739,-11365.662141,10102.361750,-1734.140410,-7102.485113,7892.361450,-84.875436,-7794.570933,7160.806037,1556.679778,-9912.874481,11244.150235,-6606.471795,1866.763535
|
||||
-59.997344,-3057.096504,14481.763570,-33663.212013,48336.047139,-42098.001548,11849.908920,24695.737503,-40829.250174,24183.970704,12635.151121,-42842.546295,48837.473212,-33902.447368,14555.957927,-3068.522684
|
||||
100.000000,-0.006882,-0.009535,-0.081985,-0.006197,-0.086599,0.025787,0.106096,-0.069921,0.008318,0.030489,-0.376848,0.061624,-0.128956,0.060856,-0.002952
|
||||
-0.000,-1875.411,6653.430,-11365.662,10102.362,-1734.140,-7102.485,7892.361,-84.875,-7794.571,7160.806,1556.680,-9912.874,11244.150,-6606.472,1866.764
|
||||
-59.997,-3057.097,14481.764,-33663.212,48336.047,-42098.002,11849.909,24695.738,-40829.250,24183.971,12635.151,-42842.546,48837.473,-33902.447,14555.958,-3068.523
|
||||
100.000,-0.007,-0.010,-0.082,-0.006,-0.087,0.026,0.106,-0.070,0.008,0.030,-0.377,0.062,-0.129,0.061,-0.003
|
||||
|
||||
|
@@ -1,3 +1,3 @@
|
||||
-0.000008,-1875.410798,6653.429739,-11365.662141,10102.361750,-1734.140410,-7102.485113,7892.361450,-84.875436,-7794.570933,7160.806037,1556.679778,-9912.874481,11244.150235,-6606.471795,1866.763535
|
||||
-59.997344,-3057.096504,14481.763570,-33663.212013,48336.047139,-42098.001548,11849.908920,24695.737503,-40829.250174,24183.970704,12635.151121,-42842.546295,48837.473212,-33902.447368,14555.957927,-3068.522684
|
||||
120.000000,-0.008258,-0.011442,-0.098382,-0.007437,-0.103918,0.030944,0.127315,-0.083905,0.009981,0.036587,-0.452217,0.073949,-0.154747,0.073028,-0.003542
|
||||
-0.000,-1875.411,6653.430,-11365.662,10102.362,-1734.140,-7102.485,7892.361,-84.875,-7794.571,7160.806,1556.680,-9912.874,11244.150,-6606.472,1866.764
|
||||
-59.997,-3057.097,14481.764,-33663.212,48336.047,-42098.002,11849.909,24695.738,-40829.250,24183.971,12635.151,-42842.546,48837.473,-33902.447,14555.958,-3068.523
|
||||
120.000,-0.008,-0.011,-0.098,-0.007,-0.104,0.031,0.127,-0.084,0.010,0.037,-0.452,0.074,-0.155,0.073,-0.004
|
||||
|
||||
|
@@ -1,3 +1,3 @@
|
||||
-0.000014,-3125.684664,11089.049565,-18942.770235,16837.269583,-2890.234017,-11837.475189,13153.935749,-141.459060,-12990.951555,11934.676728,2594.466297,-16521.457469,18740.250392,-11010.786326,3111.272558
|
||||
-99.995573,-5095.160841,24136.272616,-56105.353355,80560.078564,-70163.335913,19749.848200,41159.562505,-68048.750290,40306.617839,21058.585202,-71404.243825,81395.788686,-56504.078947,24259.929878,-5114.204474
|
||||
120.000000,-0.008258,-0.011442,-0.098382,-0.007437,-0.103918,0.030944,0.127315,-0.083905,0.009981,0.036587,-0.452217,0.073949,-0.154747,0.073028,-0.003542
|
||||
-0.000,-3125.685,11089.050,-18942.770,16837.270,-2890.234,-11837.475,13153.936,-141.459,-12990.952,11934.677,2594.466,-16521.457,18740.250,-11010.786,3111.273
|
||||
-99.996,-5095.161,24136.273,-56105.353,80560.079,-70163.336,19749.848,41159.563,-68048.750,40306.618,21058.585,-71404.244,81395.789,-56504.079,24259.930,-5114.204
|
||||
120.000,-0.008,-0.011,-0.098,-0.007,-0.104,0.031,0.127,-0.084,0.010,0.037,-0.452,0.074,-0.155,0.073,-0.004
|
||||
|
||||
|
@@ -1,3 +1,3 @@
|
||||
-0.000007,-1562.842332,5544.524783,-9471.385118,8418.634792,-1445.117008,-5918.737594,6576.967875,-70.729530,-6495.475777,5967.338364,1297.233149,-8260.728734,9370.125196,-5505.393163,1555.636279
|
||||
-49.997787,-2547.580420,12068.136308,-28052.676677,40280.039282,-35081.667957,9874.924100,20579.781253,-34024.375145,20153.308920,10529.292601,-35702.121913,40697.894343,-28252.039474,12129.964939,-2557.102237
|
||||
100.000003,781.414284,-2772.271926,4735.610574,-4209.323593,722.471906,2959.394584,-3288.377841,35.294844,3247.746206,-2983.638693,-648.993422,4130.425992,-4685.191554,2752.757438,-777.821092
|
||||
-0.000,-1562.842,5544.525,-9471.385,8418.635,-1445.117,-5918.738,6576.968,-70.730,-6495.476,5967.338,1297.233,-8260.729,9370.125,-5505.393,1555.636
|
||||
-49.998,-2547.580,12068.136,-28052.677,40280.039,-35081.668,9874.924,20579.781,-34024.375,20153.309,10529.293,-35702.122,40697.894,-28252.039,12129.965,-2557.102
|
||||
100.000,781.414,-2772.272,4735.611,-4209.324,722.472,2959.395,-3288.378,35.295,3247.746,-2983.639,-648.993,4130.426,-4685.192,2752.757,-777.821
|
||||
|
||||
|
@@ -1,3 +1,3 @@
|
||||
-0.000003,-781.421166,2772.262391,-4735.692559,4209.317396,-722.558504,-2959.368797,3288.483937,-35.364765,-3247.737889,2983.669182,648.616574,-4130.364367,4685.062598,-2752.696581,777.818140
|
||||
-24.998893,-1273.790210,6034.068154,-14026.338339,20140.019641,-17540.833978,4937.462050,10289.890626,-17012.187572,10076.654460,5264.646301,-17851.060956,20348.947172,-14126.019737,6064.982470,-1278.551118
|
||||
100.000002,390.703701,-1386.140730,2367.764295,-2104.664895,361.192653,1479.710185,-1644.135872,17.612462,1623.877262,-1491.804102,-324.685135,2065.243808,-2342.660255,1376.409147,-388.912022
|
||||
-0.000,-781.421,2772.262,-4735.693,4209.317,-722.559,-2959.369,3288.484,-35.365,-3247.738,2983.669,648.617,-4130.364,4685.063,-2752.697,777.818
|
||||
-24.999,-1273.790,6034.068,-14026.338,20140.020,-17540.834,4937.462,10289.891,-17012.188,10076.654,5264.646,-17851.061,20348.947,-14126.020,6064.982,-1278.551
|
||||
100.000,390.704,-1386.141,2367.764,-2104.665,361.193,1479.710,-1644.136,17.612,1623.877,-1491.804,-324.685,2065.244,-2342.660,1376.409,-388.912
|
||||
|
||||
|
@@ -1,3 +0,0 @@
|
||||
-0.000004,-937.705399,3326.714870,-5682.831071,5051.180875,-867.070205,-3551.242557,3946.180725,-42.437718,-3897.285466,3580.403018,778.339889,-4956.437241,5622.075118,-3303.235898,933.381767
|
||||
-29.998672,-1528.548252,7240.881785,-16831.606006,24168.023569,-21049.000774,5924.954460,12347.868752,-20414.625087,12091.985352,6317.575561,-21421.273148,24418.736606,-16951.223684,7277.978963,-1534.261342
|
||||
100.000002,468.845818,-1663.366969,2841.333551,-2525.596635,433.448504,1775.647065,-1972.984266,21.148938,1948.651051,-1790.171020,-389.546792,2478.280245,-2811.166515,1651.678805,-466.693836
|
||||
|
Reference in New Issue
Block a user