PidStatePID_s

Author: qqqlab

examples/11.QuadcopterAdvanced/main_quadadv.cpp

@@ -201,9 +201,9 @@ Yaw right               (CCW+ CW-)       -++-
     out.set_output(motor_outputs[3], thr);
   }else{
     // Quad mixing
-    out.set_output(motor_outputs[0], thr - pid.pitch - pid.roll - pid.yaw); //M1 Back Right CW
-    out.set_output(motor_outputs[1], thr + pid.pitch - pid.roll + pid.yaw); //M2 Front Right CCW
-    out.set_output(motor_outputs[2], thr - pid.pitch + pid.roll + pid.yaw); //M3 Back Left CCW
-    out.set_output(motor_outputs[3], thr + pid.pitch + pid.roll - pid.yaw); //M4 Front Left CW
+    out.set_output(motor_outputs[0], thr - pid.pitch.sum - pid.roll.sum - pid.yaw.sum); //M1 Back Right CW
+    out.set_output(motor_outputs[1], thr + pid.pitch.sum - pid.roll.sum + pid.yaw.sum); //M2 Front Right CCW
+    out.set_output(motor_outputs[2], thr - pid.pitch.sum + pid.roll.sum + pid.yaw.sum); //M3 Back Left CCW
+    out.set_output(motor_outputs[3], thr + pid.pitch.sum + pid.roll.sum - pid.yaw.sum); //M4 Front Left CW
   }
 }

src/bbx/bbx.cpp

@@ -271,20 +271,18 @@ void Bbx::log_pid(PidState *pid_s) {
   BinLog bl("PID");
   bl.keepFree = QUEUE_LENGTH/4; //keep 25% of queue free for other messages
   bl.TimeUS(pid_s->ts); 
-  bl.i16("rp", pid_s->roll_p * 1000, 1e-3);
-  bl.i16("ri", pid_s->roll_i * 1000, 1e-3);
-  bl.i16("rd", pid_s->roll_d * 1000, 1e-3);
-  bl.i16("roll", pid_s->roll * 1000, 1e-3);
-  bl.i16("pp", pid_s->pitch_p * 1000, 1e-3);
-  bl.i16("pi", pid_s->pitch_i * 1000, 1e-3);
-  bl.i16("pd", pid_s->pitch_d * 1000, 1e-3);
-  bl.i16("pitch", pid_s->pitch * 1000, 1e-3);
-  bl.i16("yp", pid_s->yaw_p * 1000, 1e-3);
-  bl.i16("yi", pid_s->yaw_i * 1000, 1e-3);
-  bl.i16("yd", pid_s->yaw_d * 1000, 1e-3);
-  bl.i16("yaw", pid_s->yaw * 1000, 1e-3);
-  bl.i16("throttle", pid_s->throttle * 1000, 1e-3);
-  bl.flt("debug", pid_s->debug); 
+  bl.i16("rp", pid_s->roll.p * 1000, 1e-3);
+  bl.i16("ri", pid_s->roll.i * 1000, 1e-3);
+  bl.i16("rd", pid_s->roll.d * 1000, 1e-3);
+  bl.i16("roll", pid_s->roll.sum * 1000, 1e-3);
+  bl.i16("pp", pid_s->pitch.p * 1000, 1e-3);
+  bl.i16("pi", pid_s->pitch.i * 1000, 1e-3);
+  bl.i16("pd", pid_s->pitch.d * 1000, 1e-3);
+  bl.i16("pitch", pid_s->pitch.sum * 1000, 1e-3);
+  bl.i16("yp", pid_s->yaw.p * 1000, 1e-3);
+  bl.i16("yi", pid_s->yaw.i * 1000, 1e-3);
+  bl.i16("yd", pid_s->yaw.d * 1000, 1e-3);
+  bl.i16("yaw", pid_s->yaw.sum * 1000, 1e-3);
 }
 
 //MODE - flight mode

src/cli/cli.cpp

@@ -199,40 +199,42 @@ static void cli_pah() {
 static void cli_ppid() {
   PidState pid_s;
   pid.topic.pull_last(&pid_s); //get a consistent copy
-  Serial.printf("pid.roll:%+.3f\t",pid_s.roll);
-  Serial.printf("pitch:%+.3f\t",pid_s.pitch);
-  Serial.printf("yaw:%+.3f\t",pid_s.yaw);
-  Serial.printf("debug:%f\t",pid_s.debug);
+  Serial.printf("pid.roll:%+.3f\t",pid_s.roll.sum);
+  Serial.printf("pitch:%+.3f\t",pid_s.pitch.sum);
+  Serial.printf("yaw:%+.3f\t",pid_s.yaw.sum);
 }
 
 static void cli_ppidr() {
   PidState pid_s;
   pid.topic.pull_last(&pid_s); //get a consistent copy
-  Serial.printf("pidr.roll:%+.3f\t",pid_s.roll);
-  Serial.printf("P:%+.3f\t",pid_s.roll_p);
-  Serial.printf("I:%+.3f\t",pid_s.roll_i);
-  Serial.printf("D:%+.3f\t",pid_s.roll_d);
-  Serial.printf("F:%+.3f\t",pid_s.roll_f);
+  Serial.printf("pidr.roll:%+.3f\t",pid_s.roll.sum);
+  Serial.printf("P:%+.3f\t",pid_s.roll.p);
+  Serial.printf("I:%+.3f\t",pid_s.roll.i);
+  Serial.printf("D:%+.3f\t",pid_s.roll.d);
+  Serial.printf("A:%+.3f\t",pid_s.roll.a);
+  Serial.printf("B:%+.3f\t",pid_s.roll.b);
 }
 
 static void cli_ppidp() {
   PidState pid_s;
   pid.topic.pull_last(&pid_s); //get a consistent copy
-  Serial.printf("pidp.pitch:%+.3f\t",pid.pitch);
-  Serial.printf("P:%+.3f\t",pid_s.pitch_p);
-  Serial.printf("I:%+.3f\t",pid_s.pitch_i);
-  Serial.printf("D:%+.3f\t",pid_s.pitch_d);
-  Serial.printf("F:%+.3f\t",pid_s.pitch_f);
+  Serial.printf("pidp.pitch:%+.3f\t",pid.pitch.sum);
+  Serial.printf("P:%+.3f\t",pid_s.pitch.p);
+  Serial.printf("I:%+.3f\t",pid_s.pitch.i);
+  Serial.printf("D:%+.3f\t",pid_s.pitch.d);
+  Serial.printf("A:%+.3f\t",pid_s.pitch.a);
+  Serial.printf("B:%+.3f\t",pid_s.pitch.b);
 }
 
 static void cli_ppidy() {
   PidState pid_s;
   pid.topic.pull_last(&pid_s); //get a consistent copy
-  Serial.printf("pidy.yaw:%+.3f\t",pid_s.yaw);
-  Serial.printf("P:%+.3f\t",pid_s.yaw_p);
-  Serial.printf("I:%+.3f\t",pid_s.yaw_i);
-  Serial.printf("D:%+.3f\t",pid_s.yaw_d);
-  Serial.printf("F:%+.3f\t",pid_s.yaw_f);
+  Serial.printf("pidy.yaw:%+.3f\t",pid_s.yaw.sum);
+  Serial.printf("P:%+.3f\t",pid_s.yaw.p);
+  Serial.printf("I:%+.3f\t",pid_s.yaw.i);
+  Serial.printf("D:%+.3f\t",pid_s.yaw.d);
+  Serial.printf("A:%+.3f\t",pid_s.yaw.a);
+  Serial.printf("B:%+.3f\t",pid_s.yaw.b);
 }
 
 static void cli_pout() {

src/pid/PidGizmoMF0.cpp

@@ -127,20 +127,20 @@ void PidGizmoMF0::control_Angle(bool zero_integrators) {
   integral_roll += error_roll * imu.dt;
   integral_roll = constrain(integral_roll, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup
   float derivative_roll = ahr.gx;
-  pid.roll_p = Kp_ro_pi_angle * error_roll;
-  pid.roll_i = Ki_ro_pi_angle * integral_roll;
-  pid.roll_d = -Kd_ro_pi_angle * derivative_roll;
-  pid.roll = pid.roll_p + pid.roll_i + pid.roll_d;
+  pid.roll.p = Kp_ro_pi_angle * error_roll;
+  pid.roll.i = Ki_ro_pi_angle * integral_roll;
+  pid.roll.d = -Kd_ro_pi_angle * derivative_roll;
+  pid.roll.sum = pid.roll.p + pid.roll.i + pid.roll.d;
 
   //Pitch PID
   float error_pitch = pitch_des - ahr.pitch;
   integral_pitch += error_pitch * imu.dt;
   integral_pitch = constrain(integral_pitch, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup
   float derivative_pitch = ahr.gy;
-  pid.pitch_p = Kp_ro_pi_angle * error_pitch;
-  pid.pitch_i = Ki_ro_pi_angle * integral_pitch;
-  pid.pitch_d = -Kd_ro_pi_angle * derivative_pitch;
-  pid.pitch = pid.pitch_p + pid.pitch_i + pid.pitch_d;
+  pid.pitch.p = Kp_ro_pi_angle * error_pitch;
+  pid.pitch.i = Ki_ro_pi_angle * integral_pitch;
+  pid.pitch.d = -Kd_ro_pi_angle * derivative_pitch;
+  pid.pitch.sum = pid.pitch.p + pid.pitch.i + pid.pitch.d;
 
   //Yaw PID
   if(-0.02 < rcl.yaw && rcl.yaw < 0.02) {
@@ -151,10 +151,10 @@ void PidGizmoMF0::control_Angle(bool zero_integrators) {
     float error_yaw = degreeModulus(yaw_desired - ahr.yaw);
     float desired_yawRate = error_yaw / 0.5; //set desired yawRate such that it gets us to desired yaw in 0.5 second
     float derivative_yaw = desired_yawRate - ahr.gz;
-    pid.yaw_p = Kp_yaw_angle * error_yaw;
-    pid.yaw_i = Kd_yaw_angle * derivative_yaw;
-    pid.yaw_d = 0;
-    pid.yaw = pid.yaw_p + pid.yaw_i + pid.yaw_d;
+    pid.yaw.p = Kp_yaw_angle * error_yaw;
+    pid.yaw.i = Kd_yaw_angle * derivative_yaw;
+    pid.yaw.d = 0;
+    pid.yaw.sum = pid.yaw.p + pid.yaw.i + pid.yaw.d;
 
     //update yaw rate controller
     error_yawRate_prev = 0;
@@ -164,10 +164,10 @@ void PidGizmoMF0::control_Angle(bool zero_integrators) {
     integral_yawRate += error_yawRate * imu.dt;
     integral_yawRate = constrain(integral_yawRate, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup
     float derivative_yawRate = (error_yawRate - error_yawRate_prev) / imu.dt;
-    pid.yaw_p = Kp_yaw_rate * error_yawRate;
-    pid.yaw_i = Ki_yaw_rate * integral_yawRate;
-    pid.yaw_d = Kd_yaw_rate * derivative_yawRate;
-    pid.yaw = pid.yaw_p + pid.yaw_i + pid.yaw_d;
+    pid.yaw.p = Kp_yaw_rate * error_yawRate;
+    pid.yaw.i = Ki_yaw_rate * integral_yawRate;
+    pid.yaw.d = Kd_yaw_rate * derivative_yawRate;
+    pid.yaw.sum = pid.yaw.p + pid.yaw.i + pid.yaw.d;
 
     //Update derivative variables
     error_yawRate_prev = error_yawRate;
@@ -206,30 +206,30 @@ void PidGizmoMF0::control_Rate(bool zero_integrators) {
   integral_roll += error_roll * imu.dt;
   integral_roll = constrain(integral_roll, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup
   float derivative_roll = (error_roll - error_roll_prev) / imu.dt;
-  pid.roll_p = Kp_ro_pi_rate * error_roll;
-  pid.roll_i = Ki_ro_pi_rate * integral_roll;
-  pid.roll_d = Kd_ro_pi_rate * derivative_roll;
-  pid.roll = pid.roll_p + pid.roll_i + pid.roll_d;
+  pid.roll.p = Kp_ro_pi_rate * error_roll;
+  pid.roll.i = Ki_ro_pi_rate * integral_roll;
+  pid.roll.d = Kd_ro_pi_rate * derivative_roll;
+  pid.roll.sum = pid.roll.p + pid.roll.i + pid.roll.d;
 
   //Pitch
   float error_pitch = pitchRate_des - ahr.gy;
   integral_pitch += error_pitch * imu.dt;
   integral_pitch = constrain(integral_pitch, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup
   float derivative_pitch = (error_pitch - error_pitch_prev) / imu.dt;
-  pid.pitch_p = Kp_ro_pi_rate * error_pitch;
-  pid.pitch_i = Ki_ro_pi_rate * integral_pitch;
-  pid.pitch_d = Kd_ro_pi_rate * derivative_pitch;
-  pid.pitch = pid.pitch_p + pid.pitch_i + pid.pitch_d;
+  pid.pitch.p = Kp_ro_pi_rate * error_pitch;
+  pid.pitch.i = Ki_ro_pi_rate * integral_pitch;
+  pid.pitch.d = Kd_ro_pi_rate * derivative_pitch;
+  pid.pitch.sum = pid.pitch.p + pid.pitch.i + pid.pitch.d;
 
   //Yaw, stablize on rate from GyroZ
   float error_yaw = yawRate_des - ahr.gz;
   integral_yaw += error_yaw * imu.dt;
   integral_yaw = constrain(integral_yaw, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup
   float derivative_yaw = (error_yaw - error_yaw_prev) / imu.dt; 
-  pid.yaw_p = Kp_yaw_rate * error_yaw;
-  pid.yaw_i = Ki_yaw_rate * integral_yaw;
-  pid.yaw_d = Kd_yaw_rate * derivative_yaw;
-  pid.yaw = pid.yaw_p + pid.yaw_i + pid.yaw_d;
+  pid.yaw.p = Kp_yaw_rate * error_yaw;
+  pid.yaw.i = Ki_yaw_rate * integral_yaw;
+  pid.yaw.d = Kd_yaw_rate * derivative_yaw;
+  pid.yaw.sum = pid.yaw.p + pid.yaw.i + pid.yaw.d;
 
   //Update derivative variables
   error_roll_prev = error_roll;

src/pid/PidGizmoMF1.cpp

@@ -117,21 +117,21 @@ void PidGizmoMF1::control_Angle() {
   integral_roll += Ki_ro_pi_angle * error_roll * imu.dt;
   integral_roll = constrain(integral_roll, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup
   float derivative_roll = ahr.gx;
-  pid.roll_p = Kp_ro_pi_angle * error_roll;
-  pid.roll_i = integral_roll;
-  pid.roll_d = -Kd_ro_pi_angle * derivative_roll;
-  pid.roll = pid.roll_p + pid.roll_i + pid.roll_d;
+  pid.roll.p = Kp_ro_pi_angle * error_roll;
+  pid.roll.i = integral_roll;
+  pid.roll.d = -Kd_ro_pi_angle * derivative_roll;
+  pid.roll.sum = pid.roll.p + pid.roll.i + pid.roll.d;
 
   //Pitch Angle PID
   float pitch_des = rcl.pitch * maxPitch; //Desired pitch angle, between -maxPitch and +maxPitch
   float error_pitch = pitch_des - ahr.pitch;
   integral_pitch += Ki_ro_pi_angle * error_pitch * imu.dt;
   integral_pitch = constrain(integral_pitch, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup
   float derivative_pitch = ahr.gy;
-  pid.pitch_p = Kp_ro_pi_angle * error_pitch;
-  pid.pitch_i = integral_pitch;
-  pid.pitch_d = -Kd_ro_pi_angle * derivative_pitch;
-  pid.pitch = pid.pitch_p + pid.pitch_i + pid.pitch_d;
+  pid.pitch.p = Kp_ro_pi_angle * error_pitch;
+  pid.pitch.i = integral_pitch;
+  pid.pitch.d = -Kd_ro_pi_angle * derivative_pitch;
+  pid.pitch.sum = pid.pitch.p + pid.pitch.i + pid.pitch.d;
 
   //Yaw Angle PID
   if(-0.02 < rcl.yaw && rcl.yaw < 0.02) {
@@ -141,14 +141,18 @@ void PidGizmoMF1::control_Angle() {
     //do not update desired yaw angle (keep position when got out of yaw-rate mode)
     integral_yaw_rate = 0; //reset yaw rate integrator
     control_YawRate(yawRate_des, &integral_yaw_angle); //use angle integrator
-    pid.debug = FLAG_ANGLE_YAWCENTER;
+    
+    //debug
+    pid.yaw.a = FLAG_ANGLE_YAWCENTER;
   }else{
     //stick off-center - control yaw rate
     float yawRate_des = rcl.yaw * maxYawRate; //Desired yaw rate, between -maxYawRate and +maxYawRate
     yaw_angle_desired = ahr.yaw; //set desired yaw to current yaw, the yaw angle controller will hold this value
     integral_yaw_angle = 0; //reset angle integrator
     control_YawRate(yawRate_des, &integral_yaw_rate); //use rate integrator
-    pid.debug = FLAG_ANGLE_YAW;
+
+    //debug
+    pid.yaw.a = FLAG_ANGLE_YAW;
   }
 }
 
@@ -159,10 +163,10 @@ void PidGizmoMF1::control_Rate() {
   integral_roll += Ki_ro_pi_rate * error_roll * imu.dt;
   integral_roll = constrain(integral_roll, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup
   float derivative_roll = (error_roll - error_roll_prev) / imu.dt;
-  pid.roll_p = Kp_ro_pi_rate * error_roll;
-  pid.roll_i = integral_roll;
-  pid.roll_d = Kd_ro_pi_rate * derivative_roll;
-  pid.roll = pid.roll_p + pid.roll_i + pid.roll_d;
+  pid.roll.p = Kp_ro_pi_rate * error_roll;
+  pid.roll.i = integral_roll;
+  pid.roll.d = Kd_ro_pi_rate * derivative_roll;
+  pid.roll.sum = pid.roll.p + pid.roll.i + pid.roll.d;
   error_roll_prev = error_roll; //Update derivative variable
 
   //Pitch Rate - Stabilize on rate from GyroY
@@ -171,17 +175,18 @@ void PidGizmoMF1::control_Rate() {
   integral_pitch += Ki_ro_pi_rate * error_pitch * imu.dt;
   integral_pitch = constrain(integral_pitch, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup
   float derivative_pitch = (error_pitch - error_pitch_prev) / imu.dt;
-  pid.pitch_p = Kp_ro_pi_rate * error_pitch;
-  pid.pitch_i = integral_pitch;
-  pid.pitch_d = Kd_ro_pi_rate * derivative_pitch;
-  pid.pitch = pid.pitch_p + pid.pitch_i + pid.pitch_d;
+  pid.pitch.p = Kp_ro_pi_rate * error_pitch;
+  pid.pitch.i = integral_pitch;
+  pid.pitch.d = Kd_ro_pi_rate * derivative_pitch;
+  pid.pitch.sum = pid.pitch.p + pid.pitch.i + pid.pitch.d;
   error_pitch_prev = error_pitch; //Update derivative variable
 
   //Yaw Rate
   float yawRate_des = rcl.yaw * maxYawRate; 
   control_YawRate(yawRate_des, &integral_yaw_rate);
 
-  pid.debug = FLAG_RATE;
+  //debug
+  pid.yaw.a = FLAG_RATE;
 }
 
 //Yaw Rate - Stabilize on rate from GyroZ, yawRate_des = Desired Yaw Rate, between-maxYawRate and +maxYawRate 
@@ -190,9 +195,9 @@ void PidGizmoMF1::control_YawRate(float yawRate_des, float* integral_yaw) {
   *integral_yaw += Ki_yaw_rate * error_yaw * imu.dt;
   *integral_yaw = constrain(*integral_yaw, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup
   float derivative_yaw = (error_yaw - error_yaw_prev) / imu.dt; 
-  pid.yaw_p = Kp_yaw_rate * error_yaw;
-  pid.yaw_i = *integral_yaw;
-  pid.yaw_d = Kd_yaw_rate * derivative_yaw;
-  pid.yaw = pid.yaw_p + pid.yaw_i + pid.yaw_d;
+  pid.yaw.p = Kp_yaw_rate * error_yaw;
+  pid.yaw.i = *integral_yaw;
+  pid.yaw.d = Kd_yaw_rate * derivative_yaw;
+  pid.yaw.sum = pid.yaw.p + pid.yaw.i + pid.yaw.d;
   error_yaw_prev = error_yaw;  //Update derivative variable
 }

src/pid/PidGizmoMF2.cpp

@@ -76,7 +76,7 @@ void PidGizmoMF2::setup() {
   stick = &rcl.roll;
   gyro = &ahr.gx;
   ahrs_angle = &ahr.roll;
-  state = (PidStatePIDFS_s*)(&pid.roll_p);
+  state = (PidStatePID_s*) &pid.roll;
   pid_dt = 1.0f / imu.getSampleRate();
   load_param();
   zeroIntegrators();
@@ -139,8 +139,7 @@ void PidGizmoMF2::control_axis(int axis, float setpoint) {
   state[axis].sum = state[axis].p + state[axis].i + state[axis].d;
   p[axis].error_prev = error;
 
-  //if(axis==0) pid.debug = p[axis].kp * 1000000;
-  //if(axis==0) pid.debug = state[axis].p * 1000;
-  //pid.debug = pid_dt;
-  if(axis==0) pid.debug = state[axis].p * 1000;
+  //debug
+  state[axis].a = p[axis].ki;
+  state[axis].b = pid_dt;
 }

src/pid/PidGizmoMF2.h

@@ -64,5 +64,5 @@ class PidGizmoMF2 : public PidGizmo {
     float *stick; //roll/pitch/yaw stick
     float *gyro; //gx,gy,gz
     float *ahrs_angle; //roll/pitch/yaw
-    PidStatePIDFS_s *state; //roll/pitch/yaw P,I,D,F,Sum
+    PidStatePID_s *state; //roll/pitch/yaw
 };