pid integrator clipped

software/MySrc/Flight/mixer.c

@@ -10,7 +10,13 @@ void mixing(mixer_handle_t *mixer_h, motor_output_t *motor_output){
     float yaw = ((float)mixer_h->input.yaw)* mixer_h->percentages.yaw;
 
     uint16_t temp;
-
+/*
+ * [4]-------[2]
+ * |          |
+ * |          |
+ * |          |
+ * [3]-------[1]
+ */
 
 
     motor_output->motor4 = (int16_t)((throttle + pitch + roll - yaw)); //must be motor 4 was motor 1

software/MySrc/Flight/pid.c

@@ -17,7 +17,7 @@ void  pid_controller_init(pid_handle_t *pid_h, float Kp, float Ki, float Kd, flo
     pid_h->gains.Kd = Kd;
 
     pid_h->T = T;
-
+    pid_h->tau = 0.002;
     pid_h->limits.min_output = min_output;
     pid_h->limits.max_output = max_output;
 
@@ -40,6 +40,7 @@ void  pid_controller_clear(pid_handle_t *pid_h)
     pid_h->differentiator = 0;
     pid_h->prev_measurement = 0;
     pid_h->output = 0;
+    pid_h->tau = 0;
 }
 
 int16_t pid_controller_update(pid_handle_t *pid_h, int16_t *pid, int16_t setp, int16_t measurement){
@@ -49,10 +50,19 @@ int16_t pid_controller_update(pid_handle_t *pid_h, int16_t *pid, int16_t setp, i
 
     int16_t proportional =  (int16_t )(pid_h->gains.Kp *(float) error); //p[n]
 
-    pid_h->integrator = pid_h->integrator + 0.5f * pid_h->gains.Ki * pid_h->T * ((float)error + pid_h->prev_error);             //i[n]
+    pid_h->integrator = pid_h->integrator + 0.5f * pid_h->gains.Ki * pid_h->T * ((float)error + pid_h->prev_error);//i[n]
+
+    if(pid_h->integrator > 5){
+        pid_h->integrator = 5;
+    }
+    else if(pid_h->integrator < -5){
+        pid_h->integrator = -5;
+    }
+
+    pid_h->differentiator = (2.0f * pid_h->tau - pid_h->T)*pid_h->differentiator-(2.0f * pid_h->gains.Kd * ( (float)measurement - pid_h->prev_measurement) ); //d[n]
 
-    pid_h->differentiator = -(2.0f * pid_h->gains.Kd * ( (float)measurement - pid_h->prev_measurement) + (2.0f * pid_h->tau - pid_h->T)*pid_h->differentiator); //d[n]
 
+    LOGD("proportional: %d, integrator: %f, differentiator: %f, first part diff: %f, second part diff: %f", proportional, pid_h->integrator, pid_h->differentiator,-(2.0f * pid_h->gains.Kd * ( (float)measurement - pid_h->prev_measurement),(2.0f * pid_h->tau - pid_h->T)*pid_h->differentiator) );
     *pid = (int16_t) ((float)proportional + (float) pid_h->integrator + (float)pid_h->differentiator); //u[n]
 
     //LOGD("PID output(before limited): %d", pid_h->output);

software/MySrc/Flight/pid_controller.c

@@ -12,13 +12,14 @@
 
 
 #define TAU 0.000625
-
 #define KP 0.5
 #define KI 1
 #define KD 0.5
 
 
 
+
+
 void pid_init(flight_pids_t *drone_pids){
 
     pid_controller_init(&drone_pids->pid.yaw_pid, KP, KI, KD, TAU, -500, 500, 1000, 2000);

software/MySrc/mymain.c

@@ -158,7 +158,7 @@ void mymain(void) {
                 // ----- all non-critical code goes here ----- //
                 none_blocking_delay(1000, &led_toggle_last_ms, (callback_t) LED_toggle, NULL);
                 none_blocking_delay(25, &cli_process_last_ms, (callback_t) cli_process, &cli_h);
-                none_blocking_delay(1000, &log_stats_last_ms, (callback_t) log_stats, NULL);
+//                none_blocking_delay(1000, &log_stats_last_ms, (callback_t) log_stats, NULL);
                 // ^^^^^ all non-critical code goes here ^^^^^ //
 
             }
@@ -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);