........... PID_Yaw_controller Source Code ............

========== File-Name: 20_Pid_Yaw_Controller.ino ===========

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// ++++++++++ Begin of PID-Yaw control function ++++++++++++++
//              Last Update: 02-11-2022
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// Yaw Yaw Yaw Yaw Yaw Yaw Yaw Yaw Yaw Yaw Yaw Yaw Yaw Yaw Yaw
// DeltaYaw is Rot.Angle of Drone to Target in deg. 
// Rot.Direc.is  CW => Rotation_Sign = -1 
// Rot.Direc.is CCW => Rotation_Sign = +1 
// ===========================================================


void control_pid_yaw()
// PID Controller Yaw calculations
 {
   float pid_p_yaw_gain = 0.7; // gain for gyro yaw-angle 
   float pid_i_yaw_gain = 0.2; 
//   float pid_d_yaw_gain = 0.0; 

  float pid_p_yaw_angle_error;    //, pid_p_roll_output;
  float pid_i_yaw_angle_error;    // = 0.0; 
//  float pid_d_yaw_angle_error;
  float pid_yaw_error_sum;
//  float last_pid_p_yaw_angle_error;

   int p_max_yaw_angle_error = 30;  
   int p_min_yaw_angle_error =-30;  

//   int i_min_yaw_angle_error= -45; 
//   int i_max_yaw_angle_error=  45;     
   
   int max_pid_yaw_output = 2200;
   int min_pid_yaw_output = 1000;   //1600 is servo midpoint

// P: Proportional PID error term in [degrees 0;] 
      pid_p_yaw_angle_error = DeltaYaw * Rotation_Sign; 

      if (pid_p_yaw_angle_error > p_max_yaw_angle_error) 
          pid_p_yaw_angle_error = p_max_yaw_angle_error;
      if (pid_p_yaw_angle_error < p_min_yaw_angle_error)
          pid_p_yaw_angle_error = p_min_yaw_angle_error;

//  Next to Rudder, banking is applied for direction control to target
       Bank_angle = pid_p_yaw_angle_error; 
       
//.................................................................
// I: Integral PID error term in [degrees/sec]
      pid_i_yaw_angle_error += pid_p_yaw_angle_error * pid_i_yaw_gain; 

//      if(pid_i_yaw_angle_error > max_i_yaw_angle_error) 
//         pid_i_yaw_angle_error = max_i_yaw_angle_error;
//      if(pid_i_yaw_angle_error < min_i_yaw_angle_error)
//         pid_i_yaw_angle_error = min_i_yaw_angle_error;

//      Serial.print("21: Pid_i_yaw_angle_error = ");
//      Serial.println(pid_i_yaw_angle_error,1);
//................................................................

/*
//................................................................
// D: Differential PID error term in [degrees/second]=> do not use. 
      pid_d_yaw_angle_error = GyroYawdot;  //Time derivative of GyroYaw

//    pid_d_yaw_angle_error = pid_p_yaw_angle_error - last_pid_p_yaw_angle_error;
// delen door Loop_T_secs in sec
//    last_pid_p_yaw_angle_error = pid_p_yaw_angle_error;
//................................................................
*/

  pid_yaw_error_sum =   pid_p_yaw_gain * pid_p_yaw_angle_error  
                      + pid_i_yaw_angle_error * 1;
  //                    + pid_d_yaw_gain * pid_d_yaw_angle_error;

//   Counter-Rotation to Cube  
//   pid_yaw_output = mapfloat(pid_p_yaw_angle_error,
//                    p_min_yaw_angle_error, p_max_yaw_angle_error,
//                    min_pid_yaw_output, max_pid_yaw_output);

//   Rotation direction in line with Cube
   pid_yaw_output = mapfloat(pid_yaw_error_sum,
                    p_min_yaw_angle_error, p_max_yaw_angle_error,
                    max_pid_yaw_output, min_pid_yaw_output);

     if (pid_yaw_output > max_pid_yaw_output)
         pid_yaw_output = max_pid_yaw_output;
     if (pid_yaw_output < min_pid_yaw_output)
         pid_yaw_output = min_pid_yaw_output;

//   pid_yaw_output = Moving_Average (10, pid_yaw_output);
   pid_yaw_output = Rotating_Memory(pid_yaw_output);

/*
   Serial.println();
   Serial.print("21: GyroYaw= ");
   Serial.print(GyroYaw,1);
  
   Serial.print("  21: DeltaYaw= ");
   Serial.print(DeltaYaw,1);   
   
   Serial.print("  21: Yaw Error= ");
   Serial.print(pid_p_yaw_angle_error,1);    
*/
//   Serial.print("21: Pid_Yaw_output = ");
//   Serial.print(pid_yaw_output,0);  

   Serial.println();
}

// ============================================================
// ++++++++++++ End of PID-Yaw control function +++++++++++++++
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++