constants.py - master - Logrus/CopterControl

constants.py
72 строки2 KB
Nikolay Nosorev
Initial commit
04 окт 2025, 16:17
04 окт 2025, 16:1799ef28c
# Физические константы
g = 9.81  # Ускорение свободного падения
m = 1.0   # Масса квадрокоптера (кг)
m_p = 0.125  # Масса пропеллера (кг)
l = 0.2   # Длина луча (м)
rc = 0.25   # Радиус квадрокоптера
k = 1e-5  # Коэффициент тяги винта
b = 1e-6 # Коэффициент момента сопротивления

dt = 0.01
# параметры (в начале файла)
OMEGA_MAX = 800.0
F_MAX = 4 * k * OMEGA_MAX**2
F_MIN = 0.0
ACC_MAX = 15.0

Kp_pos_xy = 3.01#0.75#0.75#0.5#0.5#0.1
Kd_pos_xy = 0.50#1.75#1.6#1.5#0.0#0.2

Kp_pos_z = 2.54#1.0#0.3
Ki_pos_z = 0.45#0.15#0.01
Kd_pos_z = 0.50#0.5  # обычно 0 в внешнем контуре при каскаде

# Угловой контур (самый важный!)
#kp_phi, kp_theta, kp_psi,kp_ang = 8.0, 8.0, 6.0,7.0
#kd_phi, kd_theta, kd_psi,kd_ang = 2.3, 2.3, 2.2,0.3
kp_phi, kp_theta, kp_psi = 8.26,8.48,5.92#8.0, 8.0, 6.0
kd_phi, kd_theta, kd_psi = 1.37,0.50,0.70#2.3, 2.3, 2.2

# скоростной контур (vel -> acc_cmd)
Kp_vel_xy = 2.85#2.5
Kd_vel_xy = 1.83#0.8
Kp_vel_z = 1.45#3.0
Kd_vel_z = 0.68#0.5

# Позиционный контур
#Kp_pos_xy = 1.2
#Kp_pos_z = 1.5
#Ki_pos_z = 0.1

# Фильтры
d_filter_alpha = 0.95
alpha = 0.8
int_z_min, int_z_max = -1.0, 1.0

# anti-windup и фильтр
int_z = 0.0
int_z_min = -3.5
int_z_max = 3.5


# Kp_vel_xy = 1.5#2.5#1.5
# Kd_vel_xy = 1.65#4.0#1.5#0.6

# Kp_vel_z = 2.25#3.0
# Kd_vel_z = 1.75#0.8



# Kp_vel_xy = 2.5
# Kd_vel_xy = 0.8
# Kp_vel_z = 3.0
# Kd_vel_z = 0.5



# d_filter_alpha = 0.9#0.9
#alpha=0.9#pos_desired_filtered = alpha * pos_desired_prev + (1-alpha) * pos_desired_current

# inner angle gains (existing)
#kp_phi, kp_theta, kp_psi,kp_ang = 0.6,0.6,0.6,0.6#Kp_ang
#kd_phi, kd_theta, kd_psi,kd_ang = 1.8,1.8,1.8,1.8#Kd_ang