From 5a4a3d1c04257805f2703c45ea5ed16b72b7e79b Mon Sep 17 00:00:00 2001 From: Teppen108 <131367354+Teppen108@users.noreply.github.com> Date: Fri, 31 May 2024 16:36:43 +0200 Subject: [PATCH] pid changes --- software/MySrc/Drivers/imu.c | 2 +- software/MySrc/Flight/mixer.c | 18 +++++++++--------- software/MySrc/Flight/pid_controller.c | 11 +++++++---- software/MySrc/mymain.c | 6 +++--- 4 files changed, 20 insertions(+), 17 deletions(-) diff --git a/software/MySrc/Drivers/imu.c b/software/MySrc/Drivers/imu.c index 7f5b4d1..2c24505 100644 --- a/software/MySrc/Drivers/imu.c +++ b/software/MySrc/Drivers/imu.c @@ -306,7 +306,7 @@ static int8_t set_accel_gyro_config(struct bmi2_dev *bmi) { /* NOTE: The user can change the following configuration parameters according to their requirement. */ /* Set Output Data Rate */ - config[ACCEL].cfg.acc.odr = BMI2_ACC_ODR_200HZ; + config[ACCEL].cfg.acc.odr = BMI2_ACC_ODR_1600HZ; /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ config[ACCEL].cfg.acc.range = BMI2_ACC_RANGE_2G; diff --git a/software/MySrc/Flight/mixer.c b/software/MySrc/Flight/mixer.c index 8884f77..e96a6f9 100644 --- a/software/MySrc/Flight/mixer.c +++ b/software/MySrc/Flight/mixer.c @@ -2,7 +2,7 @@ void mixing(mixer_handle_t *mixer_h, motor_output_t *motor_output){ int16_t motor_low_limit = 200; - int16_t motor_high_limit = 1000; + int16_t motor_high_limit = 1850; float throttle = ((float)mixer_h->input.throttle)*mixer_h->percentages.throttle; float roll = ((float)mixer_h->input.roll)* mixer_h->percentages.roll; @@ -13,10 +13,10 @@ void mixing(mixer_handle_t *mixer_h, motor_output_t *motor_output){ - motor_output->motor1 = (int16_t)((throttle + roll- pitch - yaw)); - motor_output->motor2 = (int16_t)((throttle - roll - pitch + yaw)); - motor_output->motor3 = (int16_t)((throttle - roll + pitch - yaw)); - motor_output->motor4 = (int16_t)((throttle + roll + pitch + yaw)); + motor_output->motor4 = (int16_t)((throttle + roll- pitch - yaw)); //must be motor 4 was motor 1 + motor_output->motor3 = (int16_t)((throttle - roll - pitch + yaw)); //must be motor 3 was motor 2 + motor_output->motor1 = (int16_t)((throttle - roll + pitch - yaw)); //must be motor 1 was motor 3 + motor_output->motor2 = (int16_t)((throttle + roll + pitch + yaw)); //must be motor 2 was motor 4 if(motor_output->motor1 > motor_high_limit){ temp = motor_output->motor1 - motor_high_limit; @@ -128,8 +128,8 @@ void mixing(mixer_handle_t *mixer_h, motor_output_t *motor_output){ } void init_mixer_percentages(mixer_handle_t *mixer_h){ - mixer_h->percentages.roll = 0.5; - mixer_h->percentages.pitch = 0.5; - mixer_h->percentages.yaw = 0.5; - mixer_h->percentages.throttle = 0.85; + mixer_h->percentages.roll = 0.8; + mixer_h->percentages.pitch = 0.8; + mixer_h->percentages.yaw = 0.8; + mixer_h->percentages.throttle = 0.8; } \ No newline at end of file diff --git a/software/MySrc/Flight/pid_controller.c b/software/MySrc/Flight/pid_controller.c index e3332ab..fe9f1f3 100644 --- a/software/MySrc/Flight/pid_controller.c +++ b/software/MySrc/Flight/pid_controller.c @@ -11,7 +11,10 @@ - +#define TAU 0.00065 +#define KD 0.6 +#define KI 0.4 +#define KP 1.2 @@ -19,9 +22,9 @@ void pid_init(flight_pids_t *drone_pids){ - pid_controller_init(&drone_pids->pid.yaw_pid, (float).6, (float)0.5, (float)0.5, (float)0.05, -500, 500, 1000, 2000); - pid_controller_init(&drone_pids->pid.roll_pid, (float).6, (float)0.5, (float)0.5, (float)0.05, -500, 500, 1000, 2000); - pid_controller_init(&drone_pids->pid.pitch_pid, (float).6, (float)0.5, (float)0.5, (float)0.05, -500, 500, 1000, 2000); + pid_controller_init(&drone_pids->pid.yaw_pid, KP, KI, KD, TAU, -500, 500, 1000, 2000); + pid_controller_init(&drone_pids->pid.roll_pid, KP, KI, KD, TAU, -500, 500, 1000, 2000); + pid_controller_init(&drone_pids->pid.pitch_pid, KP, KI, KD, TAU, -500, 500, 1000, 2000); limits_init(&drone_pids->setp, drone_pids->pid.yaw_pid.limits.min_output, drone_pids->pid.yaw_pid.limits.max_output); limits_init(&drone_pids->setp, drone_pids->pid.roll_pid.limits.min_output, drone_pids->pid.roll_pid.limits.max_output); diff --git a/software/MySrc/mymain.c b/software/MySrc/mymain.c index 01049d7..170c351 100644 --- a/software/MySrc/mymain.c +++ b/software/MySrc/mymain.c @@ -220,11 +220,11 @@ static void flight_ctrl_cycle(void) { s_points.roll_set_point.sp = set_point_calculation(&pids_h.setp, channel_data.roll, (float) 0.1, (float) 0.2); s_points.pitch_set_point.sp = set_point_calculation(&pids_h.setp, channel_data.pitch, (float) 0.1, (float) 0.2); s_points.yaw_set_point.sp = set_point_calculation(&pids_h.setp, channel_data.yaw, (float) 0.1, (float) 0.2); - + LOGD("Setpoints: roll=%d pitch=%d yaw=%d", s_points.roll_set_point.sp, s_points.pitch_set_point.sp, s_points.yaw_set_point.sp); // update pid controllers set_pids(&pids_h, &imu_data, &s_points, &pid_vals); - + LOGD ("pid vals: yaw=%d roll=%d pitch=%d, throttle = %d", pid_vals.yaw_pid, pid_vals.roll_pid, pid_vals.pitch_pid, channel_data.thr); // update motor mixer motor_mixer_h.input.yaw = pid_vals.yaw_pid; @@ -234,7 +234,7 @@ static void flight_ctrl_cycle(void) { mixing(&motor_mixer_h, &motor_output); - // LOGD("motor output: motor1=%d motor2=%d motor3=%d motor4=%d", motor_output.motor1, motor_output.motor2,motor_output.motor3, motor_output.motor4); + LOGD("motor output: motor1=%d motor2=%d motor3=%d motor4=%d", motor_output.motor1, motor_output.motor2,motor_output.motor3, motor_output.motor4); motor_set_throttle(&motors_h, 1, motor_output.motor1); motor_set_throttle(&motors_h, 2, motor_output.motor2); motor_set_throttle(&motors_h, 3, motor_output.motor3);