//=============================================================
//******* Function that calculates Pitch, Roll and Yaw ********
// Last Up-Date : 08-08-2022
//=============================================================
void Calc_Pitch_Roll_Yaw()
{
short acc[7]; //to store acc, gyro, temp readings
short mag[3];
float CaliMag[3], CaliMagh[3];
float FilterAccel_Xout_1, FilterAccel_Yout_1, FilterAccel_Zout_1;
float FilterGyro_Xout_1, FilterGyro_Yout_1, FilterGyro_Zout_1;
float FilterMag_Xout_1, FilterMag_Yout_1, FilterMag_Zout_1;
float FilterAccel_Xout_2, FilterAccel_Yout_2, FilterAccel_Zout_2;
float FilterGyro_Xout_2, FilterGyro_Yout_2, FilterGyro_Zout_2;
float FilterMag_Xout_2, FilterMag_Yout_2, FilterMag_Zout_2;
float GyroPitchdot; //, GyroPitch; AccPitch, //, MixPitch;
float GyroRolldot; // , GyroRoll; AccRoll, //, MixRoll;
float GyroYawdot; //, GyroYaw; MagYaw, GeoMagYaw, //, MixYaw; GeoYaw is MagYaw corrected for declination
float GyroX_rate, GyroY_rate, GyroZ_rate;
float AccRaw_av[3];
float R_1;
float RadAccRoll;
float RadAccPitch;
float Calib_Acc_h[3];
float Alfa_Accel;
// float Alfa_Mag;
float Alfa_Gyro;
//....... End Declaration of LOCAL Variables tilt compensated Compass .........
// For value INITIALISATION read the Accel en Gyro x/y/z from MPU6050
readMPU6050(acc);
FilterAccel_Xout_1 = (float)acc[0];
FilterAccel_Yout_1 = (float)acc[1];
FilterAccel_Zout_1 = (float)acc[2];
FilterGyro_Xout_1 = (float)acc[4]- GyroX_Drift;
FilterGyro_Yout_1 = (float)acc[5]- GyroY_Drift;
FilterGyro_Zout_1 = (float)acc[6]- GyroZ_Drift;
// For value INITIALISATION read the Magnetometer x/y/z from MAG3110
read_Mag_Data(mag);
FilterMag_Xout_1 = (float)mag[0];
FilterMag_Yout_1 = (float)mag[1];
FilterMag_Zout_1 = (float)mag[2];
//===============================================================
//--- Start STEP 1: SENSORDATA READING -----
// Note: a Low-Pass Filter is applied
FilterAccel_Xout_2 = 0;
FilterAccel_Yout_2 = 0;
FilterAccel_Zout_2 = 0;
FilterGyro_Xout_2 = 0;
FilterGyro_Yout_2 = 0;
FilterGyro_Zout_2 = 0;
FilterMag_Xout_2 = 0;
FilterMag_Yout_2 = 0;
FilterMag_Zout_2 = 0;
// Filter weightfactor Alfa's =1 means low-pass filter is switched off
Alfa_Accel = 0.5;
n = 4; //n= loop-counter
for (i = 1; i < n ; i++)
{
readMPU6050(acc); // read the globals Accel en Gyro x/y/z from MPU6050
// A weighted average LOW-pass filter is applied to the accel.data.
FilterAccel_Xout_1 = (1-Alfa_Accel)* FilterAccel_Xout_1 + Alfa_Accel * (float)acc[0];
FilterAccel_Xout_2 += FilterAccel_Xout_1;
FilterAccel_Yout_1 = (1-Alfa_Accel)* FilterAccel_Yout_1 + Alfa_Accel * (float)acc[1];
FilterAccel_Yout_2 += FilterAccel_Yout_1;
FilterAccel_Zout_1 = (1-Alfa_Accel)* FilterAccel_Zout_1 + Alfa_Accel * (float)acc[2];
FilterAccel_Zout_2 += FilterAccel_Zout_1;
//...............................................................
// A weighted average LOW-pass filter is applied to the gyro data.
Alfa_Gyro = 0.5;
FilterGyro_Xout_1 = (1-Alfa_Gyro)* FilterGyro_Xout_1 + Alfa_Gyro * ((float)acc[4]- GyroX_Drift);
FilterGyro_Xout_2 += FilterGyro_Xout_1;
FilterGyro_Yout_1 = (1-Alfa_Gyro)* FilterGyro_Yout_1 + Alfa_Gyro * ((float)acc[5]- GyroY_Drift);
FilterGyro_Yout_2 += FilterGyro_Yout_1;
FilterGyro_Zout_1 = (1-Alfa_Gyro)* FilterGyro_Zout_1 + Alfa_Gyro * ((float)acc[6]- GyroZ_Drift);
FilterGyro_Zout_2 += FilterGyro_Zout_1;
}
//...............................................................
// No weighting is applied to the LOW-pass filter of the gyro data.
m = 2; //m= loop-counter
for (i = 1; i < m ; i++)
{
read_Mag_Data(mag); // read the globals Earth-Mag x/y/z for kompass
FilterMag_Xout_2 += (float)mag[0];
FilterMag_Yout_2 += (float)mag[1];
FilterMag_Zout_2 += (float)mag[2];
}
//--- End STEP 1: SENSORDATA READING ---
//===============================================================
//--- Start STEP 2: SENSORDATA PROCESSING AND CALIBRATION ---
//===============================================================
// Convert MPU6050 Accel Bitvalues to g with the sensitivity factor
n = n - 1; //Note this has been checked with loop analysing sketch!!!!!!!!!!!!!!!!
AccRaw_av[0] = (FilterAccel_Xout_2 / n) /AccSensitivity; //Average accelerometer value in g
AccRaw_av[1] = (FilterAccel_Yout_2 / n) /AccSensitivity; //Average accelerometer value in g
AccRaw_av[2] = (FilterAccel_Zout_2 / n) /AccSensitivity; //Average accelerometer value in g
MatrixMathMultiply((float*)CaliMatrix, (float*)AccRaw_av, 3, 3, 1, Calib_Acc_h);
Calib_Acc_x = Calib_Acc_h[0] + CaliVec[0];
Calib_Acc_y = Calib_Acc_h[1] + CaliVec[1];
Calib_Acc_z = Calib_Acc_h[2] + CaliVec[2];
Gyroraw[0]=FilterGyro_Xout_2 / n;
Gyroraw[1]=FilterGyro_Yout_2 / n;
Gyroraw[2]=FilterGyro_Zout_2 / n;
GyroX_rate = Gyroraw[0] / DigitPerdps; //deg/sec
GyroY_rate = Gyroraw[1] / DigitPerdps; //deg/sec
GyroZ_rate = Gyroraw[2] / DigitPerdps; //deg/sec
m = m - 1;
FilterMag_Xout_2 = FilterMag_Xout_2 / m;
FilterMag_Yout_2 = FilterMag_Yout_2 / m;
FilterMag_Zout_2 = FilterMag_Zout_2 / m;
// float RawMag_Vec = sqrt(FilterMag_Xout_2*FilterMag_Xout_2+FilterMag_Yout_2*FilterMag_Yout_2+FilterMag_Zout_2*FilterMag_Zout_2);
// First step Magnetometer Calibration with Shift vector VS
// Note: The MAG3110 option to apply the calibration shift-vector
// on the sensor chip has been used.
CaliMag[0] = FilterMag_Xout_2;
CaliMag[1] = FilterMag_Yout_2;
CaliMag[2] = FilterMag_Zout_2;
// Second step Magnetometer Calibration with rotation matrix Wmin1
MatrixMathMatrixVectorMultiply((float*) Wmin1, (float*) CaliMag ,3,3, (float*) CaliMagh); //Use CTp here
//mmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmm
// Note: Do align here the sensor axis x,y,z with the X,Y,Z axis of the wooden cube!!
// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
// Alignment Signs for MAG3110 no.1 and no.2
FilterMag_Xout_2 = -1 * CaliMagh[1];
FilterMag_Yout_2 = CaliMagh[0];
FilterMag_Zout_2 = -1 * CaliMagh[2]; //for the z-axis definition see fig.2 of the MAG3110 Data-Sheet
// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
//--- End STEP 2: SENSORDATA PROCESSING AND CALIBRATION ---
//==========================================================
//--- START STEP 3: PITCH, ROLL and YAW CALCULATIONS ---
//============================================================
//Note: Loop_T_millis is def.global and measured in the Main-Loop
//Note: Loop_T_secs is def.global and measured in the Main-Loop
//Note: Job_T_secs is def.global and measured in the Main-Loop
// Calculation of Gravity, Roll and Pitch from the calibrated accelerometer data
// Note Pitch and Roll work for +/- 90 degrees OK met de Kubus 28-1-2017.
G_Value = sqrt(Calib_Acc_x * Calib_Acc_x + Calib_Acc_y * Calib_Acc_y + Calib_Acc_z * Calib_Acc_z);
// For below formula see Ref.8 pg.10, Eq.26
R_1 = sqrt(Calib_Acc_y * Calib_Acc_y + Calib_Acc_z * Calib_Acc_z);
// RadAccRoll = atan(Calib_Acc_y / Calib_Acc_z); //Note: atan(RADIANS) gives correct sign
RadAccRoll =-1* atan2(Calib_Acc_y, -Calib_Acc_z);
// RadAccRoll = atan2(Calib_Acc_y, Calib_Acc_z); ??????????????????????
AccRoll = RadAccRoll * 57.32- Roll_Correction; //Degrees, 180/pi=57.32 max.+/-90 degrees
RadAccPitch = atan2((-Calib_Acc_x), R_1);
AccPitch = RadAccPitch* 57.32 - Pitch_Correction; //Degrees, max.+/-90 degrees
//nnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnn
// ----- Calculation of the tilt compensated magnetic yaw -----
// Note: To get the MagYaw-angle right!!
RadAccPitch= -1*RadAccPitch;
Mag_Bfx = ((FilterMag_Xout_2) * cos(RadAccPitch))+
((FilterMag_Yout_2) * sin(RadAccPitch) * sin(RadAccRoll))+
((FilterMag_Zout_2) * sin(RadAccPitch) * cos(RadAccRoll));
Mag_Bfy = (FilterMag_Zout_2 * sin(RadAccRoll)) -
(FilterMag_Yout_2 * cos(RadAccRoll));
MagYaw = -1* atan2(-Mag_Bfy , Mag_Bfx) * 57.32; //Degrees, 57.32=180/pi
// If the compass heading becomes outside the +/-180 range, +/-360 is added to keep it in +/-180 range
if (MagYaw < -180) MagYaw += 360;
else
if (MagYaw >= 180) MagYaw -= 360;
// MagYaw = Rotating_Memory(MagYaw);
MagYaw = MagYaw - Yaw_Correction; //Yaw_Correction set in 5
// GeoMagYaw is Magyaw corrected for Declination.
// When GeoMagYaw=0 "Drone-Nose" points to Geo-North=True-North.
GeoMagYaw = MagYaw + Mag_Declination; //Mag_Declination set in 5
//-------------------------------------------------------------
// Calculation of Gyro_Pitch, Gyro_Roll and Gyro_Yaw from the gyro angular rates.
GyroPitchdot = -GyroY_rate*cos(RadAccRoll) + GyroZ_rate*sin(RadAccRoll); //deg/sec
GyroRolldot = GyroX_rate - tan(RadAccPitch)*(GyroY_rate*sin(RadAccRoll)+GyroZ_rate*cos(RadAccRoll)); //deg/sec
GyroYawdot = (1/cos(RadAccPitch)) * (GyroY_rate*sin(RadAccRoll) + GyroZ_rate*cos(RadAccRoll)); //deg/sec
GyroPitch += GyroPitchdot * Loop_T_secs; // Gyro Pitch angle in degrees
GyroRoll += GyroRolldot * Loop_T_secs; // Gyro Roll angle in degrees
GyroYaw += GyroYawdot * Loop_T_secs; // Gyro Yaw angle in Degrees
if (GyroYaw < -180) GyroYaw += 360;
else
if (GyroYaw >= +180) GyroYaw -= 360;
// Initializing Gyro Pitch, Roll and Yaw at start
// With the availability of AccPitch, AccRoll and GeoYaw
// the initial values of GyroPitch, GyroRoll and GyroYaw can be set
// Job_T_secs is Cum Loop-time in sec
if (Gyro_Init_setting == 1 && Job_T_secs > 2) //&& is logical AND
{
GyroPitch = AccPitch; // degrees
GyroRoll = AccRoll; // degrees
GyroYaw = GeoMagYaw; // degrees
Gyro_Init_setting = 2; //switch Value
}
//...........................................................
// Homing-in the Gyro-Values, blink the white LED
if (abs(G_Value-Gvalue) < 0.08) // && (Loop_T_millis < 100)) //These conditions will switch on/off Homing-In
{
GyroPitch = AccPitch; // degrees
GyroRoll = AccRoll; // degrees
GyroYaw = GeoMagYaw; // degrees corrected for mag.declination
// GyroYaw = MoveAvgMagYaw; // degrees
// strcpy(HomeIn_Switch, "Acc "); //Accelerometer is active
// Binking White LED on PCA9685 shield pin 2 OFF
pwm.setPWM(PCApin[2], 0, 4096); //OFF
}
else
{
// strcpy(HomeIn_Switch, "Gyro"); //Gyro is active
// Binking White LED on PCA9685 shield pin 2 ON
pwm.setPWM(PCApin[2], 4096, 0); //ON
}
//--- END STEP 3: Pitch, Roll and Yaw calculation
}
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+++++ End Function that calculates Pitch, Roll and Yaw +++++
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++