//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+++++++++++ Start function Heading Calculation ++++++++++++++
// Last Update: 13-02-22
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Note: The 3D Global Cartesian coordinate systeem x,y,z
// is chosen such that the x-axis lays in the equator plane.
// Its origin starts at the center of the earth, and runs throuhg
// the projection of Greenwhich (Lon=0 deg) to the equator plane
// The z-axis has also its origin in the center of the earth,
// runs through the Geographic North.
// Lon_Loc1_deg =>Actual GPS Lon-Angle of Location 1 in degrees
// Lat_Loc1_deg =>Actual GPS Lat-Angle of Location 1 in degrees
// Lon_Loc2_deg =>Google GPS Lon-Angle of Location 2 in degrees
// Lat_Loc2_deg =>Google GPS Lat-Angle of Location 2 in degrees
// The Heading Angle Alpha, is calculated with the Scalar product of
// vector from Loc1 to Geograph-North and vector from Loc1 to Loc2.
void calculateheading(float Lon_Loc1_deg, float Lat_Loc1_deg,
float Lon_Loc2_deg, float Lat_Loc2_deg)
{
int h1 = 1; //scale factor
float h_PI = 3.14159; //26535897932384626433832795
float R_Earth= 6371000; //Radius of Earth in m
// 3D Coordinates of the Geographic North-pole
// float x_N = 0.0;
// float y_N = 0.0;
float z_N = R_Earth; // 6371000 Radius of Earth in m;
// Location 1 is the Actual Point as reported by GPS
float Lon_Loc1_rad; // Lon of Location 1 in rad
float Lat_Loc1_rad; // Lat of Location 1 in rad
//Global Cartesian coordinates of Location 1
float x_Loc1; //m
float y_Loc1; //m
float z_Loc1; //m
// Location 2 is the Target Point as specified by user
float Lon_Loc2_rad; // Lon Location 2 in rad
float Lat_Loc2_rad; // Lat Location 2 in rad
//Global Cartesian coordinates of Location 2
float x_Loc2; //m
float y_Loc2; //m
float z_Loc2; //m
// Scalar product of vector from Loc1 to Geograph-North and
// vector from Loc1 to Loc2
float Loc1_NdotLoc1_Loc2;
// Length of vector from Loc1 to Geograph-North
float lenLoc1_N;
// Length of vector from Loc1 to Loc2
float lenLoc1_Loc2;
// _Alpha is angle between vector Loc1_N and vector Loc1_Loc2
float _Alpha;
// char Rotation1[5] = " CW";
// char Rotation2[5] = "CCW";
// Serial.println(GPS_val_Lat,6);
// Serial.println(GPS_val_Lon,6);
// Serial.println(Lat_Loc1_deg,6);
// Serial.println(Lon_Loc1_deg,6);
// Serial.println(Lat_Loc2_deg,6);
// Serial.println(Lon_Loc2_deg,6);
// delay(5000);
// ***Calculations***Calculations***Calculations***Calculations***
Lon_Loc1_rad = Lon_Loc1_deg * h_PI/180; //Lon of Location 1 in rad
Lat_Loc1_rad = Lat_Loc1_deg * h_PI/180; //Lat of Location 1 in rad
//Conversion of Loc1 sphere-coordinates into global cartesian coordinates x,y,z
x_Loc1 = R_Earth * cos(Lat_Loc1_rad) * cos(Lon_Loc1_rad); //m
y_Loc1 = R_Earth * cos(Lat_Loc1_rad) * sin(Lon_Loc1_rad); //m
z_Loc1 = R_Earth * sin(Lat_Loc1_rad); //m
Lon_Loc2_rad = Lon_Loc2_deg * h_PI/180; //Lon of Location 2 in rad
Lat_Loc2_rad = Lat_Loc2_deg * h_PI/180; //Lat of Location 2 in rad
//Conversion of Loc2 sphere-coordinates into global cartesian coordinates x,y,z
x_Loc2 = R_Earth * cos(Lat_Loc2_rad) * cos(Lon_Loc2_rad); //m
y_Loc2 = R_Earth * cos(Lat_Loc2_rad) * sin(Lon_Loc2_rad); //m
z_Loc2 = R_Earth * sin(Lat_Loc2_rad); //m
// Calculation Distance Loc1 to Geographic North in [m]
lenLoc1_N = sqrt(x_Loc1*x_Loc1 +y_Loc1*y_Loc1 +(z_N-z_Loc1)*(z_N-z_Loc1));
lenLoc1_N = lenLoc1_N/h1;
// Calculation Distance Loc1 to Loc2 in [m]
Flight_Distance = sqrt((x_Loc2-x_Loc1)*(x_Loc2-x_Loc1)+
(y_Loc2-y_Loc1)*(y_Loc2-y_Loc1)+
(z_Loc2-z_Loc1)*(z_Loc2-z_Loc1));
// .................................................................
if (Flight_Distance <= Max_Flight_Distance) // [m]
{
// *** Start Heading-angle calculation with 3D Scalar Vector Product ***
lenLoc1_Loc2 = Flight_Distance/h1;
// Note: For the scalar product a coordinate transformation of the
// vectors Loc1_N and Loc1_Loc2 has to be performed.
// The global coordinate values x,y,z of the vectors must be
// transformed to values in the local catesian coordinates
// system originating at Loc1.
int h2 = h1*h1; //scale factor
Loc1_NdotLoc1_Loc2 = -x_Loc1*(x_Loc2-x_Loc1)/h2 -y_Loc1*(y_Loc2-y_Loc1)/h2 +
(z_N-z_Loc1)*(z_Loc2-z_Loc1)/h2;
_Alpha = acos(Loc1_NdotLoc1_Loc2/(lenLoc1_N*lenLoc1_Loc2)) *180/h_PI;
_Alpha =abs(_Alpha);
// Note: Lon_Loc2 is Lon of Target
// When Lon_Loc2 > Lon_Loc1 then Alpha is neg. when running CW +0deg @ North to -180 deg @ South
// When Lon_Loc2 < Lon_Loc1 then Alpha is pos. when running CCW -0deg @ North to -180 deg @ South
// Note: Coordinate System is right-handed
if (Lon_Loc2_deg >= Lon_Loc1_deg)
{
Alpha = -1* _Alpha; // 0Alpha>-180 degrees
}
// **** End Heading-angle Alpha calculation with 3D Scalar Vector Product *****
// Note: Old_Values of Alpha are required for course continuation in case GPS reading fails
Old_Distance=Flight_Distance;
Old_Alpha=Alpha; // Angle between North and Target
Old_Rotation_Sign=Rotation_Sign;
// Flag=2;
// float Old_DeltaYaw; // Drone Rotation Angle to align Drone-Nose to Target
} // end if Distance <= Max_Flight_Distance
// .........................................................
// In case of erroneous GPS-Data reading such that:
if ((Flight_Distance > Max_Flight_Distance) )
{
// Serial.println();
// Serial.print("14 Heading Calc: Target is beyond reach! ! ! ! ! ! !");
// Serial.println();
// Serial.println();
if (Flag > 10) //beyond Take-Off Phase, and altitude above 200m
{
Flight_Distance = Old_Distance;
Alpha = Old_Alpha; // Angle between North and Target
Rotation_Sign = Old_Rotation_Sign;
// float Old_DeltaYaw; // Drone Rotation Angle to align Drone-Nose to Target
}
}
}
//=============================================================
//+++++++++++ End function Heading Calculation ++++++++++++++
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++