.. Heading Rotation Angle CW or CCW Calculation Source Code ..

======= File-Name: 16_Heading_Rot_Angle_CW_or_CCW.ino ========

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++++++ Start Calculation Rotation Angle and Direction ++++++
//            Last Update Bruno Best 1-10-22
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// The Heading Angle Alpha, is calculated from the Scalar product of
// vector from Loc1 to Geograph-North and vector from Loc1 to Loc2.
// GyroYaw is the angle relative to Geograph-North. DeltaYaw and its 
// rotation sign are calculated from the angels Alpha and GyroYaw.
// CCW => Rotation Sign = +1
// CW  => Rotation Sign = -1
// DeltaYaw and its relevant Rotation Sign are used by the PID-Yaw
// Controller to guide the Drone to the Target.

void Rot_Angle_CW_or_CCW()
  {
//...............................................................
       if (Loc2_Lon > Loc1_Lon) 
        {
//          Serial.print("15 Lon-LocT > Lon-LocA ");
          if (GyroYaw < 0 && GyroYaw > Alpha)
            {
              DeltaYaw=abs(Alpha)-abs(GyroYaw);
              Rotation_Sign=-1;
//             strcpy(Rotation, "1a->CW");
            }
          
          if (GyroYaw > -180 && GyroYaw < Alpha)
            {
              DeltaYaw=abs(GyroYaw)-abs(Alpha);
              Rotation_Sign=+1;
//             strcpy(Rotation, "2a->CCW");
            }
          
           if (GyroYaw < 180 && GyroYaw > 180+Alpha)
            {
              DeltaYaw=360-abs(Alpha)-(GyroYaw);
              Rotation_Sign=+1;
//             strcpy(Rotation, "3a->CCW");
            }

           if (GyroYaw > 0 && GyroYaw < 180+Alpha)
            {
              DeltaYaw=abs(Alpha)+(GyroYaw);
              Rotation_Sign=-1;
//              strcpy(Rotation, "4a->CW");
            }
        }

//..............................................................
        if (Loc2_Lon <= Loc1_Lon)
         {
 //          Serial.print("15 Lon-LocT < Lon-LocA");
           if (GyroYaw > 0 && GyroYaw < Alpha)
            {
              DeltaYaw=(Alpha)-(GyroYaw);
              Rotation_Sign=+1;
//              strcpy(Rotation, "1b->CCW");
            }

           if (GyroYaw < +180 && GyroYaw > Alpha)
            {
              DeltaYaw=(GyroYaw)-(Alpha);
              Rotation_Sign=-1;
//              strcpy(Rotation, "2b->CW");
            }
                    
           if (GyroYaw > -180 && GyroYaw < -180+Alpha)
            {
              DeltaYaw=360-Alpha-abs(GyroYaw);
              Rotation_Sign=-1;
//              strcpy(Rotation, "3b->CW");
            }
            
           if (GyroYaw < 0 && GyroYaw > -180+Alpha)
            {
              DeltaYaw=(Alpha)+abs(GyroYaw);
              Rotation_Sign=+1;
//              strcpy(Rotation, "4b->CCW");
            }
         }

    if (Rotation_Sign==-1) strcpy(Rotation, "CW");
    if (Rotation_Sign==+1) strcpy(Rotation, "CCW");
   }
//=============================================================

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+++++++ End Calculation Rotation Angle and Direction ++++++++
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++