// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// ++++++++++++++++++++++ Begin Main Loop ++++++++++++++++++++++
// Last Update 22-10-2022
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//-(10: START MAIN LOOP )--(10: START MAIN LOOP )--(10: START MAIN LOOP )
//-(10: START MAIN LOOP )--(10: START MAIN LOOP )--(10: START MAIN LOOP )
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
void loop()
{
// char GPS_Data;
time1 = millis(); //Measure Loop Start-Time in milliseconds
// Serial.println();
// Serial.print("10 Flag= ");
// Serial.println(Flag);
//Note: Flag=0 is set in 2 Set_Up Section
//Note: Loop_Step=0 is set in Set_Up Section
// .........................................................
if (Flag == 0)
{
// int GPS_Read_Freq = 0;
// As initial Actual GPS position a Position in Pacific Ocean is selected
// i.e.GPS_val_Lon = 155.14 degrees Long
// i.e.GPS_val_Lat = 44.71 degrees Lat
// Read_GPSdata((char*) GPSstrBuffer, MaxChars);
// Now the actual GPS_val_Lon and GPS_val_Lat have
// to be derived from read GPSstrBuffer and override initial values.
ProcessGPS((char*) GPSstrBuffer, nCount);
Loc1_Lon = GPS_val_Lon; // Actual GPS Lon-reading
Loc1_Lat = GPS_val_Lat; // Actual GPS Lat-reading
Loc2_Lon = Lon_angle_Home; // In Set-Up Specified GPS Lon-target
Loc2_Lat = Lat_angle_Home; // In Set-Up Specified GPS Lat-target
calculateheading(Loc1_Lon, Loc1_Lat, Loc2_Lon, Loc2_Lat);
// Flight_Distance = Distance;
if (Flight_Distance > Max_Flight_Distance)
{
Serial.println();
Serial.print("10: Distance is beyond reach = ");
Serial.print(Flight_Distance,0);
Serial.println(" m !!!!!!!!!!");
Serial.println();
for (n=0; n<5; n++)
{
// Switch-ON the Red control LED
pwm.setPWM(PCApin[11], 4096, 0); //Red
delay(600); // For synchronisation has to be a multiple of the 60Hz freq.
}
Flag=0; //has to be 0
}
else
{
// Switch-OFF the RED an GREEN control LEDs
pwm.setPWM(PCApin[11], 0, 4096); //Red LED
pwm.setPWM(PCApin[0], 0, 4096); //Green LED
delay(60); // For synchronisation has to be a multiple of the 60Hz freq.
Serial.print("10: Flight_Distance = ");
Serial.print(Flight_Distance,0);
Serial.println(" m ");
Flag=10;
delay(3000);
Loop_Control_T_Secs = 0;
Loop_T_millis = 0;
}
}
// ..............................................................
// ** Flag=10 Take-Off ** Flag=10 Take-Off ** Flag=10 Take-Off **
// ..............................................................
// Flag=10: All sensors are checked OK, Take-Off.
// - Green LED switched off
// - Red LED switched off
// Drone altitude is based on BMP280 baro-pressure;
if (Flag == 10) //altitude below 200m
{
Serial.println("10: Flag=10 Take-Off Phase");
// - Activate pid-roll for Aelerons control.
control_pid_roll(0.0); //Keep drone roll to 0
// - Set rudder in neutral position with pid_yaw_output
pid_yaw_output = 1600; //Rudder put in neutral position
// - Activate pid-pitch control and set Elevator in up-position.
control_pid_pitch(200, 50); //drone set_altitude is 200m, range +50m
// Open throttle fully for take-off.
// During test phase with transmitter (or potmeter)
// Note: Emergency-Control throttle with transmitter is enabled.
if (drone_altitude >200)
{Flag = 20;}
else
{Flag = 10;}
}
// .................................................................
// ******** Flag=20: Directional Climing and Cruise to Target ******
// .................................................................
if (Flag == 20) //drone altitude is above 200m
{
Serial.println("10: Flag=20 Directional Climing and Cruise to Target");
// - Activate pid-pitch control with Elevator.
control_pid_pitch(800, 50); //drone altitude is set to 800m, range +50m
// - Activate Yaw-Control
GPS_Read_Freq += 1;
if (GPS_Read_Freq == 20)
{
// Read_GPSdata((char*) GPSstrBuffer, MaxChars);
ProcessGPS((char*) GPSstrBuffer, nCount);
GPS_Read_Freq = 0;
}
Loc1_Lon = GPS_val_Lon;
Loc1_Lat = GPS_val_Lat;
Loc2_Lon = Lon_angle_Target;
Loc2_Lat = Lat_angle_Target;
// Serial.print("Lon_Loc1= ");
// Serial.print(Loc1_Lon,6);
// Serial.print(" ");
// Serial.print("Lat_Loc1= ");
// Serial.println(Loc1_Lat,6);
// Serial.print("Lon_Loc2= ");
// Serial.print(Loc2_Lon,6);
// Serial.print(" ");
// Serial.print("Lat_Loc2= ");
// Serial.println(Loc2_Lat,6);
calculateheading(Loc1_Lon, Loc1_Lat, Loc2_Lon, Loc2_Lat);
Rot_Angle_CW_or_CCW();
Serial.print("10: Rudder-Angle = ");
Serial.print(Alpha,1);
Serial.println(" deg");
control_pid_yaw();
// - Activate pid-roll for Aelerons control.
// Apply controlled banking for target tracking.
control_pid_roll(Bank_angle);
Serial.print("10: Rotation direction is : ");
Serial.println(Rotation);
Serial.print("10: Flight-Distance= ");
Serial.print(Flight_Distance,0);
Serial.println(" m");
int Step=1;
if (Fake_Flight_Distance >= 9) Step=1;
Fake_Flight_Distance = Fake_Flight_Distance - Step; // For testing only!!!
Serial.print("10: Fake Flight-Distance= ");
Serial.print(Fake_Flight_Distance);
Serial.println(" m");
Serial.println();
// Keep throttle fully open for Directional Climing.
// During test phase with transmitter (or potmeter)
// Note: Emergency-Control throttle with transmitter is enabled.
if (Fake_Flight_Distance < 10)
{Flag = 30;}
else
{Flag = 20;}
}
// ....................................................................
// *** Flag=30 Spiral-Down and Land ** Flag=30 Spiral-Down and Land ***
// ....................................................................
if (Flag == 30)
{
Serial.println("10: Flag=30 Spiral-Down Phase");
// - Activate pid-roll for Aelerons control.
control_pid_roll(30.0); //Keep drone roll to 30deg for spiral-Down
// - Set rudder in 30deg position with pid_yaw_output for spiral-Down
pid_yaw_output = 1000; //Rudder deflection
// - Activate pid-pitch control and set Elevator in Down-position.
pid_pitch_output = 800; //fully set fix in down-position.
// Close throttle with transmitter (or potmeter) fully
// Note: Emergency-Control throttle with transmitter is enabled.
if (drone_altitude <= 40) //altitude at or below 40m
{
Serial.println("10: Start Landing Procedure");
// Close throttle fully. During testing, use Emergency-Control with transmitter
Emrev_up_Motor = 1600; //Throttle is closed.
// - Activate pid-roll for Aelerons control.
control_pid_roll(-30.0); //Counter-act spiral-Down with ailerons
// - Set rudder in -30deg position to counter-act spiral-Down
pid_yaw_output = 2200; //Rudder deflection
// - Activate pid-pitch control and set Elevator in Neutral-position.
pid_pitch_output = 1600;
}
}
//...........................................................
// ==========================================================
//...........................................................
// - Measure baro-pressure for drone-altitude.
drone_altitude = get_baro_altitude();
Serial.print("10: Drone-Altitude = ");
Serial.print(drone_altitude,2);
Serial.println(" m");
Serial.println();
Read_GPSdata((char*) GPSstrBuffer, MaxChars);
Calc_Pitch_Roll_Yaw();
Servo_Control();
if (Loop_T_millis <= 55)
{
//Blinking YELLOW LED on PCA9685 shield pin 6
pwm.setPWM(PCApin[6], 4096, 0); //OFF
pwm.setPWM(PCApin[6], 0, 4096); //ON
}
else
{
//Blinking RED LED on PCA9685 shield pin 11
pwm.setPWM(PCApin[11], 4096, 0); //OFF
pwm.setPWM(PCApin[11], 0, 4096); //ON
}
Loop_T_millis = millis() - time1; // Loop-Time in millis
Job_T_millis += Loop_T_millis; // Cumulative time in millis since start of loop. Has to be type float
Loop_T_secs = ((float)(Loop_T_millis)/1000); // Loop-time in sec
Job_T_secs += Loop_T_secs; // Cum Loop-time in sec
Loop_Control_T_Secs += Loop_T_secs; // Cum Loop-time in sec
printfreq += 1;
if (printfreq == 2) //was 20
{
// Printfunction();
printfreq = 0;
}
Serial.print("10 Loop_T_millis = ");
Serial.println(Loop_T_millis);
Loop_T_millis = 0;
} // End of Main-Loop
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// --( END MAIN LOOP )--( END MAIN LOOP )--( END MAIN LOOP )--
// --( END MAIN LOOP )--( END MAIN LOOP )--( END MAIN LOOP )--
// --( END MAIN LOOP )--( END MAIN LOOP )--( END MAIN LOOP )--
// --( END MAIN LOOP )--( END MAIN LOOP )--( END MAIN LOOP )--
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++