bisultanov/RaDrone
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

save

Browse Source
This commit is contained in:
Раджаб Бисултанов
2026-03-23 12:31:59 +03:00
parent c3a7907e16
commit 8e69ab9cdc
2 changed files with 16 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
Source/BSP/Src/imu_processing.c
View File

@@ -24,9 +24,9 @@ void imu_processing_init()
biquad_init_lpf(&accel_y_lpf, 25.0f, 500.0f); biquad_init_lpf(&accel_y_lpf, 25.0f, 500.0f);
biquad_init_lpf(&accel_z_lpf, 25.0f, 500.0f); biquad_init_lpf(&accel_z_lpf, 25.0f, 500.0f);
biquad_init_lpf(&gyro_x_lpf, 100.0f, 500.0f); biquad_init_lpf(&gyro_x_lpf, 25.0f, 500.0f);
biquad_init_lpf(&gyro_y_lpf, 100.0f, 500.0f); biquad_init_lpf(&gyro_y_lpf, 25.0f, 500.0f);
biquad_init_lpf(&gyro_z_lpf, 100.0f, 500.0f); biquad_init_lpf(&gyro_z_lpf, 25.0f, 500.0f);
} }
void imu_read_scaled(imu_scaled_t* out) void imu_read_scaled(imu_scaled_t* out)

26
Source/Control/Src/attitude.c
View File

@@ -10,13 +10,13 @@ static Vector3 gyro_bias = {};
volatile uint8_t imu_update_flag = 0; volatile uint8_t imu_update_flag = 0;
volatile uint8_t pid_update_flag = 0; volatile uint8_t pid_update_flag = 0;
float angle_kp_roll = 2.5f;
float angle_kp_pitch = 2.5f; float angle_kp_pitch = 2.5f;
float angle_kp_roll = 2.5f;
float angle_kp_yaw = 2.0f; float angle_kp_yaw = 2.0f;
pid_t pid_roll = {.kp = 0.6f, .ki = 0.0f, .kd = 0.025f};
pid_t pid_pitch = {.kp = 0.6f, .ki = 0.0f, .kd = 0.025f}; pid_t pid_pitch = {.kp = 0.6f, .ki = 0.0f, .kd = 0.025f};
pid_t pid_yaw = {.kp = 1.8f, .ki = 0.6f, .kd = 0.0f}; pid_t pid_roll = {.kp = 0.6f, .ki = 0.0f, .kd = 0.025f};
pid_t pid_yaw = {.kp = 0.6f, .ki = 0.0f, .kd = 0.0f};
/*int16_t desired_roll = 0; /*int16_t desired_roll = 0;
int16_t desired_pitch = 0; int16_t desired_pitch = 0;
@@ -51,7 +51,7 @@ void attitude_estimator_update(attitude_t* att, const imu_scaled_t* imu)
float norm = sqrtf(accel.x*accel.x + accel.y*accel.y + accel.z*accel.z); float norm = sqrtf(accel.x*accel.x + accel.y*accel.y + accel.z*accel.z);
if (norm > 0.0001f) if (norm > 0.00000000001f)
{ {
accel.x /= norm; accel.x /= norm;
accel.y /= norm; accel.y /= norm;
@@ -104,8 +104,8 @@ void attitude_controller_update(control_channels_t* control, const rc_channels*
Vector3 desired_rate = Vector3 desired_rate =
{ {
angle_error.x * angle_kp_roll, angle_error.x * angle_kp_pitch,
angle_error.y * angle_kp_pitch, angle_error.y * angle_kp_roll,
angle_error.z * angle_kp_yaw angle_error.z * angle_kp_yaw
}; };
@@ -113,24 +113,24 @@ void attitude_controller_update(control_channels_t* control, const rc_channels*
desired_rate.y = constrain(desired_rate.y, -RATE_LIM, RATE_LIM); desired_rate.y = constrain(desired_rate.y, -RATE_LIM, RATE_LIM);
desired_rate.z = constrain(desired_rate.z, -RATE_LIM, RATE_LIM); desired_rate.z = constrain(desired_rate.z, -RATE_LIM, RATE_LIM);
float roll_error = desired_rate.x - att->gyro.x; float pitch_error = desired_rate.x - att->gyro.x;
float pitch_error = desired_rate.y - att->gyro.y; float roll_error = desired_rate.y - att->gyro.y;
float yaw_error = desired_rate.z - att->gyro.z; float yaw_error = desired_rate.z - att->gyro.z;
control->roll = pid_update(&pid_roll, roll_error, att->gyro.x, IMU_DT); control->pitch = pid_update(&pid_pitch, pitch_error, att->gyro.x, IMU_DT);
control->pitch = pid_update(&pid_pitch, pitch_error, att->gyro.y, IMU_DT); control->roll = pid_update(&pid_roll, roll_error, att->gyro.y, IMU_DT);
control->yaw = pid_update(&pid_yaw, yaw_error, att->gyro.z, IMU_DT); control->yaw = pid_update(&pid_yaw, yaw_error, att->gyro.z, IMU_DT);
} }
Quaternion rx_to_quaternion(const rc_channels* rx) Quaternion rx_to_quaternion(const rc_channels* rx)
{ {
float roll = int_mapping(rx->rc_roll, -500, 500, -45, 45) * DEG2RAD;
float pitch = int_mapping(rx->rc_pitch, -500, 500, -45, 45) * DEG2RAD; float pitch = int_mapping(rx->rc_pitch, -500, 500, -45, 45) * DEG2RAD;
float roll = int_mapping(rx->rc_roll, -500, 500, -45, 45) * DEG2RAD;
float yaw = 0; float yaw = 0;
Vector3 rpy = {roll, pitch, yaw}; Vector3 pry = {pitch, roll, yaw};
return QuatCreateFromEuler(&rpy); return QuatCreateFromEuler(&pry);
} }
Vector3 gravity_from_quat(const Quaternion* q) Vector3 gravity_from_quat(const Quaternion* q)