pid changes

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;

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;
 }

software/MySrc/Flight/pid_controller.c

@@ -11,17 +11,20 @@
 
 
 
-
+#define TAU 0.00065
+#define KD 0.6
+#define KI 0.4
+#define KP 1.2
 
 
 
 
 
 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);

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);