//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++++++++++++++ Begin Flight Control Function ++++++++++++++++
//++ General Comments, Libraries, Constants and Global Vars ++
// Last Update: 15-10-2022
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Note: decimal is a POINT and not a KOMMA. < is < operator.
// Flight-Control based on sketch of a Tilt Compensated Compass with GPS and air-pressure sensor
// DUE-Teensy-DUE-Teensy-DUE-Teensy-DUE-Teensy-DUE-Teensy-DUE-Teensy-
// with Calibrated MPU6050 (accel & gyro) and Magnetometer MAG3110.
// Results displayed on monitor only.
// This code will control 4 Servo's with servo-library.
// The Pitch, Roll, Yaw is controlled bij accel., gyro, and magnetometer sensors.
// The formula's for yawdot, pitchdot and rolldot as function of gyro data are included
// All used for input to the PID-Controller.
// Homing-in is applied.
// "Pelles-ST-Rotated-Ellipsoid-Fitting-for-Sensor-Calibration" is applied for Magnetometer calibration.
// - PCA9685 will be used for control LED's.
// - PCA9685 GRD to Arduino Ground.
// - PCA9685 VCC to 3.3V of Arduino.
// - PCA9685 PWM output 0,1,2 etc. to be used for System Control-Leds
// - PCA9685 GRD output (black) pins to be connected to the LEDs-LEDS
// -GPS-GPS-GPS-GPS-GPS-GPS-GPS-GPS-GPS-GPS-GPS-GPS-GPS-GPS-GPS
// Serial1 (pin 18 en 19) is being used to read GPS
// Connect VCC of GPS board to 3.3V of Due
// Connect Ground of GPS board to Ground of Due
// Connect RX of GPS board to Serial1 TX1 (18) of Due
// Connect TX of GPS board to Serial1 RX1 (19) of Due
// GPS sketch represents the coordinates as floats
#include
#include
//#include
#include
#include // with DUE all Wire has to be replaced by Wire1
#define I2C Wire //Teensy
#include
// Call Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
// I2C address PCA9685 is: 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);
#define MPU_addr (0x68) // I2C Device address as specified in data sheet
#define KOMPASS (0x0E) // I2C address of MAG3110.
#define BMP280_Addr (0x76) // BMP-280 I2C address is 0x76
//...................................................................
// Begin Coordinate-Axis Offset address def.MAG3110 sensor
#define OFF_X_MSB 0x09
#define OFF_X_LSB 0x0A
#define OFF_Y_MSB 0x0B
#define OFF_Y_LSB 0x0C
#define OFF_Z_MSB 0x0D
#define OFF_Z_LSB 0x0E
#define SPKR 9 // 9 is the digital pin to plug the speaker "red" wire in
unsigned long time1; //millis() returns an unsigned long, type int
int Loop_T_millis; //Loop-time in millis
float Loop_T_secs;
int Job_T_millis = 0;
float Loop_Control_T_Secs;
float Job_T_secs = 0.0; //sec
int GPS_Read_Freq = 19;
int nDevices = 1; //This value should be >=4 when the correct number of I2C sensors are found.
int Flag;
int Gyro_Init_setting = 1;
// mmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmm
// Select the processor number that will be used
// mmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmm
// Processor= 1: Switch for Test Bench Teensy and Calibration Data of Test-Bench Sensors.
// Processor= 2: Switch for Drone Teensy and Calibration Data of Drone Sensors.
int Processor = 1; //Select Test-Bench Teensy
// int Processor = 2; //Select Drone Teensy
// mmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmm
// mmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmm
int Calib_Serie;
float Gvalue; //G-value is set in Calibration Matrices Function
// PCA9685 output pins for 16 control LED's
uint8_t PCApin[] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15};
float GyroX_Drift = 0;
float GyroY_Drift = 0;
float GyroZ_Drift = 0;
float AccSensitivity; // Accel-Sensitivity LSB/g
float DigitPerdps; // gyro-Sensitivity Digits per deg.per.second
float CaliMatrix[3][3]; // Accelerator Calibratie Matrix
float CaliVec[3]; // Accelerator Calibratie shift vector
short Mag_VSx, Mag_VSy, Mag_VSz; // Magnetometer Calibratie Shift vector
float Wmin1[3][3]; //Magnetometer calibration matrix.
int n, m;
int i = 0, j = 0;
int start=0;
//======================================================
//======================================================
//--- Ultrasone and Barometric Altitude Sensors Vars ---
int AltitudeFlag=0;
float drone_altitude;
float homing_in_altitude;
int hswitch = 0;
// ................................................................................................
// Temp. Calibration data from barometric BMP280 EEProm
uint16_t dig_T1;
int16_t dig_T2, dig_T3;
// Press. Calibration data from barometric BMP280 EEProm
uint16_t dig_P1;
int16_t dig_P2,dig_P3,dig_P4,dig_P5,dig_P6,dig_P7,dig_P8,dig_P9;
// Barometric Altitude Sensor Variables
int count;
uint16_t Coff[12];
byte data[6];
int32_t t_fine;
float temperature;
float start_baropressure; //millibar
float actual_baropressure; //millibar
float baro_height; //[cm] max.value=2.147.483.647cm
// ======================================================
// ======================================================
float Loc1_Lon, Loc1_Lat, Loc2_Lon, Loc2_Lat;
// ++++++++++++++++++ GPS global data declaration ++++++++++++++++++
// ++++++++++++++++++ GPS global data declaration ++++++++++++++++++
const int MaxChars = 128; //128 or 256 characters is sufficient for a hit
char GPSstrBuffer[MaxChars]= {'\0'};
size_t nCount;
int GPS_Flag =0;
// As initial Actual GPS position a Position in Pacific Ocean is selected
float GPS_val_Lon = 155.14; // GPS reading in degrees Long
float GPS_val_Lat = 44.71; // GPS reading in degrees Lat
// ++++++++ Heading global data declaration ++++ Heading global data declaration ++++++
float Lon_angle_Home; // User defined in degrees Long
float Lat_angle_Home; // User defined in degrees Lat
//char _HomeName[15];
String HomeName;
float Lon_angle_Target = 0.00; // User defined in degrees Long
float Lat_angle_Target = 0.00; // User defined in degrees Lat
//char _TargetName[15];
String TargetName;
// float Flight_Distance;
int Fake_Flight_Distance = 1500; //m use for testing only
int Max_Flight_Distance = 10000; //[m]
// Set as initial value for Flight_Distance=2*Max_Flight_Distance
float Flight_Distance; // =2*Max_Flight_Distance;
float Alpha; // Angle between North and Target
float DeltaYaw; // Drone Rotation Angle to align Drone-Nose to Target
char Rotation[5]; // Rotation direction CW or CWW to align Drone-Nose to Target
int Rotation_Sign;
int Old_Distance;
float Old_Alpha; // Angle between North and Target
float Old_DeltaYaw; // Drone Rotation Angle to align Drone-Nose to Target
int Old_Rotation_Sign;
// ======= PID Control Variables, GAIN and LIMIT SETTINGS =======
// .........................................................
// PID-ROLL ----- PID-ROLL ----- PID-ROLL ----- PID-ROLL ---
float pid_roll_output;
// .............................................................
// PID-PITCH ----- PID-PITCH ----- PID-PITCH ----- PID-PITCH ---
float pitch_setpoint;
float pid_pitch_output;
//.........................................................
// PID-YAW ----- PID-YAW ----- PID-YAW ----- PID-YAW -----
float Bank_angle =0.0;
float pid_yaw_output;
// ============= Begin global variables to Calc_Pitch_Roll_Yaw =========
float AccPitch, AccRoll;
float GyroPitch, GyroRoll;
float MagYaw, GeoMagYaw, GyroYaw;
float G_Value;
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
float Calib_Acc_x, Calib_Acc_y, Calib_Acc_z;
float Mag_Bfx, Mag_Bfy, Mag_Bfy1, Mag_Bfy2;
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// ============= End global variables to Calc_Pitch_Roll_Yaw =========
float Pitch_Correction;
float Roll_Correction;
float Yaw_Correction;
float Mag_Declination;
//...................................................................................
//Moving-average calculation
int const len = 10; //number of elements used for averaging
float arrNumbers[len] = {0}; //declaration Array with len elementa.
float CumulVals = 0;
float MoveAver = 0;
//.......................................................................
//Rotating-average calculation
int const Max_RotateIndex = 10;
float RotateArray[Max_RotateIndex] = {0}; //declaration Array with Rotate_MaxIndex elementa.
int Rotate_Index = 0;
float Cum_Value = 0;
float RotateAvg = 0;
//...................................................................................
//Gyro Calibration
float Gyroraw[3];
float GyroCalib[3];
float GyroVs[3]; //Gyro Calibration shift Vector
float GyroCaliMx[3][3]; //Gyro Calibration Matrix
// ........Servo-Control Variables ........Serv-Control Variables .........Motor-Control Variables.....
// create the servo's as "servo"Objekts for control.
Servo Servo_0, Servo_1, Servo_2, Servo_3;
// Servo_0 => Ailerons
// Servo_1 => Hoogte roer
// Servo_2 => Rudder
// Servo_3 => free
int PWMpin_4_Servo_0 = 2; // Teensy PWM pin2 4=for Roll Ailerons Servo
int PWMpin_4_Servo_1 = 3; // Teensy PWM pin3 4=for Pitch Elevator Servo
int PWMpin_4_Servo_2 = 4; // Teensy PWM pin4 4=for Yaw Rudder Servo
int PWMpin_4_Servo_3 = 5; // Teensy PWM pin5 4=for Motor Servo
int EmVPin14 = 14; // Teensy Pin 14 for Analog Input to read Emergency PWM-to-Voltage values
int EmVreadValue;
int EmCorrection;
int Emrev_up_Motor = 0; //Emrev is Emergency-rev-up Motor @ Frame-Pos0123
//1600 all servo's in mid-position
int Servo_0_In = 1600; // PWM Value Passed to Servo_0 via Teensy-Pin 2
int Servo_1_In = 1600; // PWM Value Passed to Servo_1 via Teensy-Pin 3
int Servo_2_In = 1600; // PWM Value Passed to Servo_2 via Teensy-Pin 4
int Servo_3_In = 1600; // PWM Value Passed to Servo_3 via Teensy-Pin 5
//----------- Serial_Print Variables List ------------------
int printfreq = 0;
//-------- End of Serial_Print Variables List --------------
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+++++++++++++++ End Flight Control Function +++++++++++++++++
//++ General Comments, Libraries, Constants and Global Vars ++
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++