Darrien Galid / Mbed 2 deprecated Fused_Boat_Program

Self-navigating boat program with sensors and control system fused

Dependencies:   mbed Servo PID

main.cpp@0:cf5854b3296f, 2019-05-10 (annotated)

Committer:
omar28744
Date:
Fri May 10 08:45:41 2019 +0000
Revision:
0:cf5854b3296f
Child:
1:736ae4695570
localization program

Who changed what in which revision?

UserRevisionLine numberNew contents of line
omar28744 0:cf5854b3296f 1 #include "mbed.h"
omar28744 0:cf5854b3296f 2 #include "MPU9250.h"
omar28744 0:cf5854b3296f 3 #include "math.h"
omar28744 0:cf5854b3296f 4 #include "TinyGPSplus.h"
omar28744 0:cf5854b3296f 5
omar28744 0:cf5854b3296f 6 #define TX5 PTD3
omar28744 0:cf5854b3296f 7 #define RX5 PTD2
omar28744 0:cf5854b3296f 8 #define GPSBaud 9600
omar28744 0:cf5854b3296f 9 #define destinate_lat 1.533166
omar28744 0:cf5854b3296f 10 #define destinate_lng 110.357162
omar28744 0:cf5854b3296f 11
omar28744 0:cf5854b3296f 12 DigitalOut led(PTD3);
omar28744 0:cf5854b3296f 13 MPU9250 mpu9250; // Instantiate MPU9250 class
omar28744 0:cf5854b3296f 14
omar28744 0:cf5854b3296f 15 Timer t; // setup a timer
omar28744 0:cf5854b3296f 16
omar28744 0:cf5854b3296f 17 Serial pc(USBTX, USBRX); // Serial communication to display resuslts on PC
omar28744 0:cf5854b3296f 18 Serial GPSSerial(TX5, RX5); //Serial communication to read data from GPS
omar28744 0:cf5854b3296f 19 TinyGPSPlus tgps; // Instantiate TinyGPS class
omar28744 0:cf5854b3296f 20
omar28744 0:cf5854b3296f 21
omar28744 0:cf5854b3296f 22 float p1,p2,p3,p4,a1,a2,a3,a4,q1i,q2i,q3i,q4i,qnorm,a2o=0,a3o=0,a4o=0,axf=0,ayf=0,azf=0,l=1.0,now_vel=0,last_vel=0;
omar28744 0:cf5854b3296f 23 uint32_t movwind=0, ready = 0,GPS_FLAG=0,axzerocount=0,ayzerocount=0,start_time=90,stop_avg=0,count_gps_reads=0,onetime=0,print_count=0;
omar28744 0:cf5854b3296f 24 char buffer[14];
omar28744 0:cf5854b3296f 25 uint8_t whoami = 0;
omar28744 0:cf5854b3296f 26 float deltat = 0.0f,ax=0.0f,ay=0.0f,az=0.0f,mx=0.0f,my=0.0f,mz=0.0f,gx=0.0f,gy=0.0f,gz=0.0f;
omar28744 0:cf5854b3296f 27 double D=0,Do = 0, H=0, prev_lat=0,perev_lng=0,GPSPNO=0,GPSPEO=0,GPSPN,GPSPNintial,GPSPEintial,GpsstartingptN,GpsstartingptE,GPSPNcurrent,GPSPEcurrent,GPSPE,GPSVNO,GPSVEO,GPSVN,GPSVE,accel_var=0.0015,GPS_VAR = 2;
omar28744 0:cf5854b3296f 28 double gps_lato=0,gps_longo=0,gps_lat=0,gps_long=0,position_filt_N,position_unfilt_N,position_filt_E,position_unfilt_E,position_to_dest_N,position_to_dest_E,dest_P_N,dest_P_E,unposition_to_dest_N ,unposition_to_dest_E;
omar28744 0:cf5854b3296f 29 double actual_pk1N = 1,actual_pk2N = 0,actual_pk3N = 0,actual_pk4N = 1,actual_pk1E = 1,actual_pk2E = 0,actual_pk3E = 0,actual_pk4E = 1,predicted_lat = 0, predicted_lng = 0,dest = 0,corrected_ang,sum_avg_lng=0,sum_avg_lat=0;
omar28744 0:cf5854b3296f 30
omar28744 0:cf5854b3296f 31 void setup(); // used in calibration of MPU9250 and setting baud rates for serial com.
omar28744 0:cf5854b3296f 32
omar28744 0:cf5854b3296f 33 void Read_MPU9250_Raw();// used in reading raw accel, gyro, magneto in 2 axis from the MPU9250 (IMU)
omar28744 0:cf5854b3296f 34
omar28744 0:cf5854b3296f 35 // used to fuse the accelerometer with gyro and magnetometer readings to determine the orientation in quaternion form
omar28744 0:cf5854b3296f 36 void MadgwickUpdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float deltat);
omar28744 0:cf5854b3296f 37
omar28744 0:cf5854b3296f 38 void compute_accel_world_ref();//Used to rotate the acceleration from the sensor fram to the world frame (NED) using quaternion
omar28744 0:cf5854b3296f 39
omar28744 0:cf5854b3296f 40 void compute_orientation();// used in computing the roll, Pitch and yaw from quaternion
omar28744 0:cf5854b3296f 41
omar28744 0:cf5854b3296f 42 void SerialInterruptHandler(void);// Interupt function where GPS data is read
omar28744 0:cf5854b3296f 43
omar28744 0:cf5854b3296f 44 double latitudetometers(double latitudetom);// convert long to meters (North) on Earth
omar28744 0:cf5854b3296f 45
omar28744 0:cf5854b3296f 46 double longitudetometers(double longitudetom);// convert long to meters (East) on Earth
omar28744 0:cf5854b3296f 47
omar28744 0:cf5854b3296f 48 // used to perform deadreckoning
omar28744 0:cf5854b3296f 49 void kalmanfilterpreditction(double &PO, double &VO, double acceleration, double &actual_pk1, double &actual_pk2, double &actual_pk3, double &actual_pk4,int azerocount);
omar28744 0:cf5854b3296f 50
omar28744 0:cf5854b3296f 51 // used to update the accumilative (IMU) readings with absolute GPS readings
omar28744 0:cf5854b3296f 52 void kalmanfilterupdate(double GPS_POS, double GPS_VEL, double &predicted_pos, double &predicted_vel, double &actual_pk1, double &actual_pk2, double &actual_pk3, double &actual_pk4);
omar28744 0:cf5854b3296f 53
omar28744 0:cf5854b3296f 54 void IMU_GPS_Fusion();// used in impleminting the Kalman filter to output predicted velocity and position.
omar28744 0:cf5854b3296f 55
omar28744 0:cf5854b3296f 56 float invSqrt(float x);// inverse square root function
omar28744 0:cf5854b3296f 57
omar28744 0:cf5854b3296f 58
omar28744 0:cf5854b3296f 59 int main()
omar28744 0:cf5854b3296f 60 {
omar28744 0:cf5854b3296f 61
omar28744 0:cf5854b3296f 62 setup();// code to setup GPS/MPU9250
omar28744 0:cf5854b3296f 63
omar28744 0:cf5854b3296f 64 /* Main Loop*/
omar28744 0:cf5854b3296f 65 while(1)
omar28744 0:cf5854b3296f 66 {
omar28744 0:cf5854b3296f 67 Read_MPU9250_Raw();
omar28744 0:cf5854b3296f 68 MadgwickUpdate(gx, gy, gz, ax, ay, az, mx, my, mz, deltat);
omar28744 0:cf5854b3296f 69 compute_accel_world_ref();
omar28744 0:cf5854b3296f 70 compute_orientation();
omar28744 0:cf5854b3296f 71
omar28744 0:cf5854b3296f 72 // GPSSerial.attach(&SerialInterruptHandler, Serial::RxIrq);// Interrupt trigerrer
omar28744 0:cf5854b3296f 73 //if(t.read_ms()/1000>(start_time-2))
omar28744 0:cf5854b3296f 74 //stop_avg=1;
omar28744 0:cf5854b3296f 75 if(t.read_ms()/1000>start_time)//90 seconds has passed in order to start taking reliable acceleration readings from the filter 90 sec = 60 calibration+30 filter stabilization
omar28744 0:cf5854b3296f 76 {
omar28744 0:cf5854b3296f 77
omar28744 0:cf5854b3296f 78 GPSSerial.attach(&SerialInterruptHandler, Serial::RxIrq);// Interrupt trigerrer
omar28744 0:cf5854b3296f 79 if(ready == 1)
omar28744 0:cf5854b3296f 80 {
omar28744 0:cf5854b3296f 81 IMU_GPS_Fusion();
omar28744 0:cf5854b3296f 82
omar28744 0:cf5854b3296f 83 //**************************************
omar28744 0:cf5854b3296f 84 //a2 //This variable is acceleration North
omar28744 0:cf5854b3296f 85 //a3 //This variable is acceleration East
omar28744 0:cf5854b3296f 86 //yaw //This variable is the yaw (Heading)
omar28744 0:cf5854b3296f 87 //corrected_ang //This variable returns the angle needed to adjust in order to reach the destination (-VE need to rotate clockwise to reach dest, +ve need to rotate C.C.W to reach dest)
omar28744 0:cf5854b3296f 88 //position_to_dest_N//This variable returns distance needed to reach the destination in North (meters).
omar28744 0:cf5854b3296f 89 //position_to_dest_E//This variable returns distance needed to reach the destination in East.
omar28744 0:cf5854b3296f 90 //position_filt_N //This variable returns how much you have moved in North (meters)
omar28744 0:cf5854b3296f 91 //position_filt_E //This variable return how much you have moved in East (meters)
omar28744 0:cf5854b3296f 92 //GPSVNO // This variable returns your velocity in North
omar28744 0:cf5854b3296f 93 //GPSEN0 //This variable returns your velocity in East
omar28744 0:cf5854b3296f 94 //*****************************************
omar28744 0:cf5854b3296f 95
omar28744 0:cf5854b3296f 96 //a4 //This variable is acceleration Down
omar28744 0:cf5854b3296f 97 //roll //This variable is the roll
omar28744 0:cf5854b3296f 98 //Pitch // This variable is the Pitch
omar28744 0:cf5854b3296f 99 //position_filt_N //This variable returns how much you have moved in North
omar28744 0:cf5854b3296f 100 //position_filt_E //This variable return how much you have moved in East
omar28744 0:cf5854b3296f 101 //GPSPNO //This variable returns your current position in the world in term of meters (North)
omar28744 0:cf5854b3296f 102 //GPSPEO //This variable returns your current position in the world in term of meters (East
omar28744 0:cf5854b3296f 103 //GpsstartingptN; //This variable returns your starting North position in terms of meters (Measured from latitude of GPS)
omar28744 0:cf5854b3296f 104 //GpsstartingptE; //This variable returns your starting position East in terms of meters (Measured from latitude of GPS)
omar28744 0:cf5854b3296f 105
omar28744 0:cf5854b3296f 106 if (print_count ==6)
omar28744 0:cf5854b3296f 107 {
omar28744 0:cf5854b3296f 108 printf ("D(VH) = %f, Angle = %f, vel = %f\n\r",sqrt((position_to_dest_N*position_to_dest_N)+(position_to_dest_E*position_to_dest_E)),corrected_ang,sqrt((GPSVNO*GPSVNO)+(GPSVEO*GPSVEO)));
omar28744 0:cf5854b3296f 109 //printf ("Fd = %.2f\n\r",sqrt((position_to_dest_N*position_to_dest_N)+(position_to_dest_E*position_to_dest_E)));
omar28744 0:cf5854b3296f 110 print_count = 0;
omar28744 0:cf5854b3296f 111 }
omar28744 0:cf5854b3296f 112 print_count++;
omar28744 0:cf5854b3296f 113 //printf("%i\n\r",count_gps_reads);
omar28744 0:cf5854b3296f 114 }
omar28744 0:cf5854b3296f 115
omar28744 0:cf5854b3296f 116 }
omar28744 0:cf5854b3296f 117
omar28744 0:cf5854b3296f 118 }
omar28744 0:cf5854b3296f 119
omar28744 0:cf5854b3296f 120 }
omar28744 0:cf5854b3296f 121
omar28744 0:cf5854b3296f 122 void MadgwickUpdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float deltat)
omar28744 0:cf5854b3296f 123 {
omar28744 0:cf5854b3296f 124 float recipNorm;
omar28744 0:cf5854b3296f 125 float s0, s1, s2, s3;
omar28744 0:cf5854b3296f 126 float qDot1, qDot2, qDot3, qDot4;
omar28744 0:cf5854b3296f 127 float hx, hy;
omar28744 0:cf5854b3296f 128 float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
omar28744 0:cf5854b3296f 129
omar28744 0:cf5854b3296f 130 // Rate of change of quaternion from gyroscope
omar28744 0:cf5854b3296f 131 qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
omar28744 0:cf5854b3296f 132 qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
omar28744 0:cf5854b3296f 133 qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
omar28744 0:cf5854b3296f 134 qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
omar28744 0:cf5854b3296f 135
omar28744 0:cf5854b3296f 136 // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
omar28744 0:cf5854b3296f 137 if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
omar28744 0:cf5854b3296f 138
omar28744 0:cf5854b3296f 139 // Normalise accelerometer measurement
omar28744 0:cf5854b3296f 140 recipNorm = invSqrt(ax * ax + ay * ay + az * az);
omar28744 0:cf5854b3296f 141 ax *= recipNorm;
omar28744 0:cf5854b3296f 142 ay *= recipNorm;
omar28744 0:cf5854b3296f 143 az *= recipNorm;
omar28744 0:cf5854b3296f 144
omar28744 0:cf5854b3296f 145 // Normalise magnetometer measurement
omar28744 0:cf5854b3296f 146 recipNorm = invSqrt(mx * mx + my * my + mz * mz);
omar28744 0:cf5854b3296f 147 mx *= recipNorm;
omar28744 0:cf5854b3296f 148 my *= recipNorm;
omar28744 0:cf5854b3296f 149 mz *= recipNorm;
omar28744 0:cf5854b3296f 150
omar28744 0:cf5854b3296f 151 // Auxiliary variables to avoid repeated arithmetic
omar28744 0:cf5854b3296f 152 _2q0mx = 2.0f * q0 * mx;
omar28744 0:cf5854b3296f 153 _2q0my = 2.0f * q0 * my;
omar28744 0:cf5854b3296f 154 _2q0mz = 2.0f * q0 * mz;
omar28744 0:cf5854b3296f 155 _2q1mx = 2.0f * q1 * mx;
omar28744 0:cf5854b3296f 156 _2q0 = 2.0f * q0;
omar28744 0:cf5854b3296f 157 _2q1 = 2.0f * q1;
omar28744 0:cf5854b3296f 158 _2q2 = 2.0f * q2;
omar28744 0:cf5854b3296f 159 _2q3 = 2.0f * q3;
omar28744 0:cf5854b3296f 160 _2q0q2 = 2.0f * q0 * q2;
omar28744 0:cf5854b3296f 161 _2q2q3 = 2.0f * q2 * q3;
omar28744 0:cf5854b3296f 162 q0q0 = q0 * q0;
omar28744 0:cf5854b3296f 163 q0q1 = q0 * q1;
omar28744 0:cf5854b3296f 164 q0q2 = q0 * q2;
omar28744 0:cf5854b3296f 165 q0q3 = q0 * q3;
omar28744 0:cf5854b3296f 166 q1q1 = q1 * q1;
omar28744 0:cf5854b3296f 167 q1q2 = q1 * q2;
omar28744 0:cf5854b3296f 168 q1q3 = q1 * q3;
omar28744 0:cf5854b3296f 169 q2q2 = q2 * q2;
omar28744 0:cf5854b3296f 170 q2q3 = q2 * q3;
omar28744 0:cf5854b3296f 171 q3q3 = q3 * q3;
omar28744 0:cf5854b3296f 172
omar28744 0:cf5854b3296f 173 // Reference direction of Earth's magnetic field
omar28744 0:cf5854b3296f 174 hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
omar28744 0:cf5854b3296f 175 hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
omar28744 0:cf5854b3296f 176 _2bx = sqrtf(hx * hx + hy * hy);
omar28744 0:cf5854b3296f 177 _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
omar28744 0:cf5854b3296f 178 _4bx = 2.0f * _2bx;
omar28744 0:cf5854b3296f 179 _4bz = 2.0f * _2bz;
omar28744 0:cf5854b3296f 180
omar28744 0:cf5854b3296f 181 // Gradient decent algorithm corrective step
omar28744 0:cf5854b3296f 182 s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
omar28744 0:cf5854b3296f 183 s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
omar28744 0:cf5854b3296f 184 s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
omar28744 0:cf5854b3296f 185 s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
omar28744 0:cf5854b3296f 186 recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
omar28744 0:cf5854b3296f 187 s0 *= recipNorm;
omar28744 0:cf5854b3296f 188 s1 *= recipNorm;
omar28744 0:cf5854b3296f 189 s2 *= recipNorm;
omar28744 0:cf5854b3296f 190 s3 *= recipNorm;
omar28744 0:cf5854b3296f 191
omar28744 0:cf5854b3296f 192 // Apply feedback step
omar28744 0:cf5854b3296f 193 qDot1 -= beta * s0;
omar28744 0:cf5854b3296f 194 qDot2 -= beta * s1;
omar28744 0:cf5854b3296f 195 qDot3 -= beta * s2;
omar28744 0:cf5854b3296f 196 qDot4 -= beta * s3;
omar28744 0:cf5854b3296f 197 }
omar28744 0:cf5854b3296f 198
omar28744 0:cf5854b3296f 199 // Integrate rate of change of quaternion to yield quaternion
omar28744 0:cf5854b3296f 200 q0 += qDot1 * deltat;
omar28744 0:cf5854b3296f 201 q1 += qDot2 * deltat;
omar28744 0:cf5854b3296f 202 q2 += qDot3 * deltat;
omar28744 0:cf5854b3296f 203 q3 += qDot4 * deltat;
omar28744 0:cf5854b3296f 204
omar28744 0:cf5854b3296f 205 // Normalise quaternion
omar28744 0:cf5854b3296f 206 recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
omar28744 0:cf5854b3296f 207 q0 *= recipNorm;
omar28744 0:cf5854b3296f 208 q1 *= recipNorm;
omar28744 0:cf5854b3296f 209 q2 *= recipNorm;
omar28744 0:cf5854b3296f 210 q3 *= recipNorm;
omar28744 0:cf5854b3296f 211
omar28744 0:cf5854b3296f 212 }
omar28744 0:cf5854b3296f 213 float invSqrt(float x)
omar28744 0:cf5854b3296f 214 {
omar28744 0:cf5854b3296f 215 float halfx = 0.5f * x;
omar28744 0:cf5854b3296f 216 float y = x;
omar28744 0:cf5854b3296f 217 long i = *(long*)&y;
omar28744 0:cf5854b3296f 218 i = 0x5f3759df - (i>>1);
omar28744 0:cf5854b3296f 219 y = *(float*)&i;
omar28744 0:cf5854b3296f 220 y = y * (1.5f - (halfx * y * y));
omar28744 0:cf5854b3296f 221 y = y * (1.5f - (halfx * y * y));
omar28744 0:cf5854b3296f 222 return y;
omar28744 0:cf5854b3296f 223 }
omar28744 0:cf5854b3296f 224 double latitudetometers(double latitudetom)// Position in North
omar28744 0:cf5854b3296f 225 {
omar28744 0:cf5854b3296f 226 double lattom;
omar28744 0:cf5854b3296f 227 lattom = tgps.distanceBetween(latitudetom,0, 0, 0);
omar28744 0:cf5854b3296f 228 if(latitudetom<0)
omar28744 0:cf5854b3296f 229 lattom *=-1;
omar28744 0:cf5854b3296f 230 return lattom;
omar28744 0:cf5854b3296f 231 }
omar28744 0:cf5854b3296f 232 double longitudetometers(double longitudetom)// Position in East
omar28744 0:cf5854b3296f 233 {
omar28744 0:cf5854b3296f 234 double lngtom;
omar28744 0:cf5854b3296f 235 lngtom = tgps.distanceBetween(0,longitudetom, 0, 0);
omar28744 0:cf5854b3296f 236 if(longitudetom<0)
omar28744 0:cf5854b3296f 237 lngtom *=-1;
omar28744 0:cf5854b3296f 238 return lngtom;
omar28744 0:cf5854b3296f 239 }
omar28744 0:cf5854b3296f 240
omar28744 0:cf5854b3296f 241 void SerialInterruptHandler(void)
omar28744 0:cf5854b3296f 242 {
omar28744 0:cf5854b3296f 243 if(GPSSerial.readable())
omar28744 0:cf5854b3296f 244 {
omar28744 0:cf5854b3296f 245
omar28744 0:cf5854b3296f 246 if (tgps.encode(GPSSerial.getc()))
omar28744 0:cf5854b3296f 247 { gps_lat = tgps.location.lat();
omar28744 0:cf5854b3296f 248 gps_long = tgps.location.lng();
omar28744 0:cf5854b3296f 249 if((gps_lat-gps_lato !=0) || (gps_long-gps_longo !=0))
omar28744 0:cf5854b3296f 250 GPS_FLAG = 1;
omar28744 0:cf5854b3296f 251 //else
omar28744 0:cf5854b3296f 252 //{
omar28744 0:cf5854b3296f 253 //GPS_FLAG = 0;
omar28744 0:cf5854b3296f 254 // pc.printf("position filter = %f\n\r",l);
omar28744 0:cf5854b3296f 255 // }
omar28744 0:cf5854b3296f 256 gps_lato=gps_lat;
omar28744 0:cf5854b3296f 257 gps_longo=gps_long;
omar28744 0:cf5854b3296f 258 if(GPSPNO==0) //loop one time only
omar28744 0:cf5854b3296f 259 {
omar28744 0:cf5854b3296f 260 GPSPNO=latitudetometers(tgps.location.lat());
omar28744 0:cf5854b3296f 261 GPSPEO=longitudetometers(tgps.location.lng());
omar28744 0:cf5854b3296f 262 GPSVEO=0;
omar28744 0:cf5854b3296f 263 GPSVNO=0;
omar28744 0:cf5854b3296f 264 GPSPNintial =GPSPNO;// used to measure the velocity by comparing the distance moved every 1 sec
omar28744 0:cf5854b3296f 265 GPSPEintial =GPSPEO;
omar28744 0:cf5854b3296f 266 GpsstartingptN =GPSPNO;// starting point refrence
omar28744 0:cf5854b3296f 267 GpsstartingptE = GPSPEO;
omar28744 0:cf5854b3296f 268
omar28744 0:cf5854b3296f 269 }
omar28744 0:cf5854b3296f 270 else
omar28744 0:cf5854b3296f 271 ready = 1;
omar28744 0:cf5854b3296f 272 // if(ready ==1 &&GPS_FLAG == 1 && stop_avg== 0)
omar28744 0:cf5854b3296f 273 //{
omar28744 0:cf5854b3296f 274 // sum_avg_lat = sum_avg_lat+GPSPNO;
omar28744 0:cf5854b3296f 275 // sum_avg_lng = sum_avg_lng+GPSPEO;
omar28744 0:cf5854b3296f 276
omar28744 0:cf5854b3296f 277 //GPSPNO=latitudetometers(tgps.location.lat());
omar28744 0:cf5854b3296f 278 //GPSPEO=latitudetometers(tgps.location.lat());
omar28744 0:cf5854b3296f 279 //count_gps_reads++;
omar28744 0:cf5854b3296f 280 //}
omar28744 0:cf5854b3296f 281 //if(stop_avg ==1&&onetime==0)
omar28744 0:cf5854b3296f 282 //{
omar28744 0:cf5854b3296f 283 // GPSPNO=sum_avg_lat/count_gps_reads;
omar28744 0:cf5854b3296f 284 //GPSPEO=sum_avg_lng/count_gps_reads;
omar28744 0:cf5854b3296f 285 //onetime=1;
omar28744 0:cf5854b3296f 286 //}
omar28744 0:cf5854b3296f 287
omar28744 0:cf5854b3296f 288 }
omar28744 0:cf5854b3296f 289 }
omar28744 0:cf5854b3296f 290 return;
omar28744 0:cf5854b3296f 291 }
omar28744 0:cf5854b3296f 292 void kalmanfilterpreditction(double &PO, double &VO, double acceleration, double &actual_pk1, double &actual_pk2, double &actual_pk3, double &actual_pk4, int azerocount)
omar28744 0:cf5854b3296f 293 {
omar28744 0:cf5854b3296f 294 double predicted_pk1,predicted_pk2,predicted_pk3,predicted_pk4,Predicted_p,Predicted_v;
omar28744 0:cf5854b3296f 295 Predicted_p = PO+(VO*deltat)+(0.5*deltat*deltat*acceleration);
omar28744 0:cf5854b3296f 296 Predicted_v = VO+acceleration*deltat;
omar28744 0:cf5854b3296f 297 predicted_pk1 = actual_pk1 +(deltat*actual_pk2)+(deltat*actual_pk3)+(deltat*deltat*actual_pk4)+accel_var;
omar28744 0:cf5854b3296f 298 predicted_pk2 = actual_pk2 + (deltat*actual_pk4);
omar28744 0:cf5854b3296f 299 predicted_pk3 = actual_pk3 + (deltat*actual_pk4);
omar28744 0:cf5854b3296f 300 predicted_pk4 = actual_pk4 + accel_var;
omar28744 0:cf5854b3296f 301 if(azerocount>=5)
omar28744 0:cf5854b3296f 302 {
omar28744 0:cf5854b3296f 303 VO= 0;
omar28744 0:cf5854b3296f 304 }
omar28744 0:cf5854b3296f 305 else
omar28744 0:cf5854b3296f 306 {
omar28744 0:cf5854b3296f 307 VO = Predicted_v;
omar28744 0:cf5854b3296f 308 }
omar28744 0:cf5854b3296f 309 PO = Predicted_p;
omar28744 0:cf5854b3296f 310 actual_pk1 = predicted_pk1;
omar28744 0:cf5854b3296f 311 actual_pk2 = predicted_pk2;
omar28744 0:cf5854b3296f 312 actual_pk3 = predicted_pk3;
omar28744 0:cf5854b3296f 313 actual_pk4 = predicted_pk4;
omar28744 0:cf5854b3296f 314 }
omar28744 0:cf5854b3296f 315 void kalmanfilterupdate(double GPS_POS, double GPS_VEL, double &predicted_pos, double &predicted_vel, double &actual_pk1, double &actual_pk2, double &actual_pk3, double &actual_pk4)
omar28744 0:cf5854b3296f 316 {
omar28744 0:cf5854b3296f 317 double y = 1/((actual_pk1*actual_pk4+actual_pk1*GPS_VAR+actual_pk4*GPS_VAR+GPS_VAR*GPS_VAR)-(actual_pk2*actual_pk3));
omar28744 0:cf5854b3296f 318 double GK1 = (actual_pk1*actual_pk4+actual_pk1*GPS_VAR-actual_pk2*actual_pk3)*y;
omar28744 0:cf5854b3296f 319 double GK2 = (actual_pk2*GPS_VAR)*y;
omar28744 0:cf5854b3296f 320 double GK3 = (actual_pk3*GPS_VAR)*y;
omar28744 0:cf5854b3296f 321 double GK4 = (actual_pk1*actual_pk4+actual_pk4*GPS_VAR-actual_pk2*actual_pk3)*y;
omar28744 0:cf5854b3296f 322 if(predicted_vel!=0)
omar28744 0:cf5854b3296f 323 {
omar28744 0:cf5854b3296f 324 predicted_pos = predicted_pos+GK1*GPS_POS-GK1*predicted_pos+GK3*GPS_VEL-GK3*predicted_vel;
omar28744 0:cf5854b3296f 325 predicted_vel = predicted_vel+GK2*GPS_POS-GK2*predicted_pos+GK4*GPS_VEL-GK4*predicted_vel;
omar28744 0:cf5854b3296f 326 }
omar28744 0:cf5854b3296f 327 // predicted_pos = predicted_pos+GK1*GPS_POS-GK1*predicted_pos;
omar28744 0:cf5854b3296f 328 // predicted_vel = predicted_vel+GK2*GPS_POS-GK2*predicted_pos;
omar28744 0:cf5854b3296f 329 actual_pk1 = actual_pk1-actual_pk1*GK1-GK3*actual_pk2;
omar28744 0:cf5854b3296f 330 actual_pk2 = actual_pk2-actual_pk2*GK4-GK2*actual_pk1;
omar28744 0:cf5854b3296f 331 actual_pk3 = actual_pk3-actual_pk3*GK1-GK3*actual_pk4;
omar28744 0:cf5854b3296f 332 actual_pk4 = actual_pk4-actual_pk4*GK4-GK2*actual_pk3;
omar28744 0:cf5854b3296f 333 }
omar28744 0:cf5854b3296f 334
omar28744 0:cf5854b3296f 335 void setup()
omar28744 0:cf5854b3296f 336 {
omar28744 0:cf5854b3296f 337 GPSSerial.baud(GPSBaud);
omar28744 0:cf5854b3296f 338 wait(0.001);
omar28744 0:cf5854b3296f 339 pc.baud(9600);
omar28744 0:cf5854b3296f 340 myled = 0; // turn off led
omar28744 0:cf5854b3296f 341
omar28744 0:cf5854b3296f 342 wait(5);
omar28744 0:cf5854b3296f 343
omar28744 0:cf5854b3296f 344 //Set up I2C
omar28744 0:cf5854b3296f 345 i2c.frequency(400000); // use fast (400 kHz) I2C
omar28744 0:cf5854b3296f 346
omar28744 0:cf5854b3296f 347 t.start(); // enable system timer
omar28744 0:cf5854b3296f 348
omar28744 0:cf5854b3296f 349 myled = 1; // turn on led
omar28744 0:cf5854b3296f 350
omar28744 0:cf5854b3296f 351 // Read the WHO_AM_I register, this is a good test of communication
omar28744 0:cf5854b3296f 352 whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
omar28744 0:cf5854b3296f 353 pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x73\n\r");
omar28744 0:cf5854b3296f 354 myled = 1;
omar28744 0:cf5854b3296f 355
omar28744 0:cf5854b3296f 356 if (whoami == 0x73) // WHO_AM_I should always be 0x73
omar28744 0:cf5854b3296f 357 {
omar28744 0:cf5854b3296f 358 pc.printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami);
omar28744 0:cf5854b3296f 359 pc.printf("MPU9250 is online...\n\r");
omar28744 0:cf5854b3296f 360 wait(1);
omar28744 0:cf5854b3296f 361
omar28744 0:cf5854b3296f 362 mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
omar28744 0:cf5854b3296f 363
omar28744 0:cf5854b3296f 364 mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
omar28744 0:cf5854b3296f 365 pc.printf("x-axis self test: acceleration trim within: %f pct of factory value\n\r", SelfTest[1]);
omar28744 0:cf5854b3296f 366 pc.printf("y-axis self test: acceleration trim within: %f pct of factory value\n\r", SelfTest[2]);
omar28744 0:cf5854b3296f 367 pc.printf("z-axis self test: acceleration trim within: %f pct of factory value\n\r", SelfTest[2]);
omar28744 0:cf5854b3296f 368 pc.printf("x-axis self test: gyration trim within: %f pct of factory value\n\r", SelfTest[3]);
omar28744 0:cf5854b3296f 369 pc.printf("y-axis self test: gyration trim within: %f pct of factory value\n\r", SelfTest[4]);
omar28744 0:cf5854b3296f 370 pc.printf("z-axis self test: gyration trim within: %f pct of factory value\n\r", SelfTest[5]);
omar28744 0:cf5854b3296f 371
omar28744 0:cf5854b3296f 372 mpu9250.getAres(); // Get accelerometer sensitivity
omar28744 0:cf5854b3296f 373 mpu9250.getGres(); // Get gyro sensitivity
omar28744 0:cf5854b3296f 374 mpu9250.getMres(); // Get magnetometer sensitivity
omar28744 0:cf5854b3296f 375 pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
omar28744 0:cf5854b3296f 376 pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
omar28744 0:cf5854b3296f 377 pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
omar28744 0:cf5854b3296f 378
omar28744 0:cf5854b3296f 379 mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
omar28744 0:cf5854b3296f 380 pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
omar28744 0:cf5854b3296f 381 pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
omar28744 0:cf5854b3296f 382 pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
omar28744 0:cf5854b3296f 383 pc.printf("x accel bias = %f\n\r", accelBias[1]);
omar28744 0:cf5854b3296f 384 pc.printf("y accel bias = %f\n\r", accelBias[0]);
omar28744 0:cf5854b3296f 385 pc.printf("z accel bias = %f\n\r", accelBias[2]);
omar28744 0:cf5854b3296f 386 wait(2);
omar28744 0:cf5854b3296f 387
omar28744 0:cf5854b3296f 388 mpu9250.initMPU9250();
omar28744 0:cf5854b3296f 389 pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
omar28744 0:cf5854b3296f 390 wait(1);
omar28744 0:cf5854b3296f 391
omar28744 0:cf5854b3296f 392 mpu9250.initAK8963(magCalibration);
omar28744 0:cf5854b3296f 393 pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
omar28744 0:cf5854b3296f 394 pc.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale));
omar28744 0:cf5854b3296f 395 pc.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale));
omar28744 0:cf5854b3296f 396 if(Mscale == 0) pc.printf("Magnetometer resolution = 14 bits\n\r");
omar28744 0:cf5854b3296f 397 if(Mscale == 1) pc.printf("Magnetometer resolution = 16 bits\n\r");
omar28744 0:cf5854b3296f 398 if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
omar28744 0:cf5854b3296f 399 if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
omar28744 0:cf5854b3296f 400
omar28744 0:cf5854b3296f 401 pc.printf("Mag Calibration: Wave device in a figure eight until done!");
omar28744 0:cf5854b3296f 402 wait(4);
omar28744 0:cf5854b3296f 403 mpu9250.magcalMPU9250(magBias, magScale);
omar28744 0:cf5854b3296f 404 pc.printf("Mag Calibration done!\n\r");
omar28744 0:cf5854b3296f 405 pc.printf("x mag bias = %f\n\r", magBias[0]);
omar28744 0:cf5854b3296f 406 pc.printf("y mag bias = %f\n\r", magBias[1]);
omar28744 0:cf5854b3296f 407 pc.printf("z mag bias = %f\n\r", magBias[2]);
omar28744 0:cf5854b3296f 408 //predicted_pk = A*actual_pk;
omar28744 0:cf5854b3296f 409 //predicted_pk.print();
omar28744 0:cf5854b3296f 410 wait(2);
omar28744 0:cf5854b3296f 411 }
omar28744 0:cf5854b3296f 412
omar28744 0:cf5854b3296f 413 else
omar28744 0:cf5854b3296f 414
omar28744 0:cf5854b3296f 415 {
omar28744 0:cf5854b3296f 416 pc.printf("Could not connect to MPU9250: \n\r");
omar28744 0:cf5854b3296f 417 pc.printf("%#x \n", whoami);
omar28744 0:cf5854b3296f 418 myled = 0;
omar28744 0:cf5854b3296f 419
omar28744 0:cf5854b3296f 420 while(1) ; // Loop forever if communication doesn't happen
omar28744 0:cf5854b3296f 421 }
omar28744 0:cf5854b3296f 422 }
omar28744 0:cf5854b3296f 423
omar28744 0:cf5854b3296f 424 void Read_MPU9250_Raw()
omar28744 0:cf5854b3296f 425 {
omar28744 0:cf5854b3296f 426 //while(movwind<64)
omar28744 0:cf5854b3296f 427 //{
omar28744 0:cf5854b3296f 428 mpu9250.readMPU9250Data(MPU9250Data); // INT cleared on any read
omar28744 0:cf5854b3296f 429 ax = (float)MPU9250Data[1]*aRes - accelBias[1]; // get actual g value, this depends on scale being set
omar28744 0:cf5854b3296f 430 ay = (float)MPU9250Data[0]*aRes - accelBias[0];
omar28744 0:cf5854b3296f 431 az = (float)MPU9250Data[2]*aRes - accelBias[2];
omar28744 0:cf5854b3296f 432 ax = (ax)*9.80665f;// to get it in m/s^2
omar28744 0:cf5854b3296f 433 ay = (ay)*9.80665f;// to get it in m/s^2
omar28744 0:cf5854b3296f 434 az = (-az)*9.80665f;// to get it in m/s^2
omar28744 0:cf5854b3296f 435 axf=axf+ax;
omar28744 0:cf5854b3296f 436 ayf=ayf+ay;
omar28744 0:cf5854b3296f 437 azf=azf+az;
omar28744 0:cf5854b3296f 438 movwind++;
omar28744 0:cf5854b3296f 439 //}
omar28744 0:cf5854b3296f 440 axf = axf/movwind;
omar28744 0:cf5854b3296f 441 ayf=ayf/movwind;
omar28744 0:cf5854b3296f 442 azf=azf/movwind;
omar28744 0:cf5854b3296f 443 movwind = 0;
omar28744 0:cf5854b3296f 444 //pc.printf("ax = %f", axf);
omar28744 0:cf5854b3296f 445 // pc.printf(" ay = %f",ayf);
omar28744 0:cf5854b3296f 446 //pc.printf(" az = %f m/s/s\n\r",azf);
omar28744 0:cf5854b3296f 447 mpu9250.readMagData(magCount); // Read the x/y/z adc values
omar28744 0:cf5854b3296f 448 // Calculate the magnetometer values in milliGauss
omar28744 0:cf5854b3296f 449 // Include factory calibration per data sheet and user environmental corrections
omar28744 0:cf5854b3296f 450 gx = (float)MPU9250Data[5]*gRes - gyroBias[1]; // get actual gyro value, this depends on scale being set
omar28744 0:cf5854b3296f 451 gy = (float)MPU9250Data[4]*gRes - gyroBias[0];
omar28744 0:cf5854b3296f 452 gz = (float)MPU9250Data[6]*gRes - gyroBias[2];
omar28744 0:cf5854b3296f 453
omar28744 0:cf5854b3296f 454 mx = (float)magCount[0]*mRes*magCalibration[0] - magBias[0];
omar28744 0:cf5854b3296f 455 my = (float)magCount[1]*mRes*magCalibration[1] - magBias[1];
omar28744 0:cf5854b3296f 456 mz = (float)magCount[2]*mRes*magCalibration[2] - magBias[2];
omar28744 0:cf5854b3296f 457
omar28744 0:cf5854b3296f 458
omar28744 0:cf5854b3296f 459 // poor man's soft iron calibration
omar28744 0:cf5854b3296f 460 mx *= magScale[0];
omar28744 0:cf5854b3296f 461 my *= magScale[1];
omar28744 0:cf5854b3296f 462 mz *= magScale[2];
omar28744 0:cf5854b3296f 463 Now = t.read_us();
omar28744 0:cf5854b3296f 464 // Calculate the gyro value into actual degrees per second
omar28744 0:cf5854b3296f 465 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
omar28744 0:cf5854b3296f 466 lastUpdate = Now;
omar28744 0:cf5854b3296f 467
omar28744 0:cf5854b3296f 468 // Pass gyro rate as rad/s
omar28744 0:cf5854b3296f 469 mx = mx/10.0f;
omar28744 0:cf5854b3296f 470 my = my/10.0f;
omar28744 0:cf5854b3296f 471 mz = mz/10.0f;
omar28744 0:cf5854b3296f 472 gx = gx*(3.141592f/180.0f);
omar28744 0:cf5854b3296f 473 gy = gy*(3.141592f/180.0f);
omar28744 0:cf5854b3296f 474 gz = -gz*(3.141592f/180.0f);
omar28744 0:cf5854b3296f 475 }
omar28744 0:cf5854b3296f 476
omar28744 0:cf5854b3296f 477 void compute_accel_world_ref()
omar28744 0:cf5854b3296f 478 {
omar28744 0:cf5854b3296f 479 //quaternion inverse calculation
omar28744 0:cf5854b3296f 480 qnorm = q0*q0+q1*q1+q2*q2+q3*q3;
omar28744 0:cf5854b3296f 481 q1i = q0/qnorm;
omar28744 0:cf5854b3296f 482 q2i = -q1/qnorm;
omar28744 0:cf5854b3296f 483 q3i = -q2/qnorm;
omar28744 0:cf5854b3296f 484 q4i = -q3/qnorm;
omar28744 0:cf5854b3296f 485 //quaternion rotation * acceleration vector
omar28744 0:cf5854b3296f 486 p1 = q0*0-q1*ax-q2*ay-q3*az;
omar28744 0:cf5854b3296f 487 p2 = q0*ax+q1*0+q2*az-q3*ay;
omar28744 0:cf5854b3296f 488 p3 = q0*ay-q1*az+q2*0+q3*ax;
omar28744 0:cf5854b3296f 489 p4 = q0*az+q1*ay-q2*ax+q3*0;
omar28744 0:cf5854b3296f 490 // compute acceleration vector
omar28744 0:cf5854b3296f 491 a1 = p1*q1i-p2*q2i-p3*q3i-p4*q4i; // = 0
omar28744 0:cf5854b3296f 492 a2 = p1*q2i+p2*q1i+p3*q4i-p4*q3i;
omar28744 0:cf5854b3296f 493 a3 = p1*q3i-p2*q4i+p3*q1i+p4*q2i;
omar28744 0:cf5854b3296f 494 a4 = p1*q4i+p2*q3i-p3*q2i+p4*q1i;
omar28744 0:cf5854b3296f 495 a4 = a4-9.80665f;//subtract the gravity component
omar28744 0:cf5854b3296f 496 //Filtered accel
omar28744 0:cf5854b3296f 497
omar28744 0:cf5854b3296f 498 // filter used to eliminate micromovements from filter readings
omar28744 0:cf5854b3296f 499 if((a2<=0.042)&&a2>=-0.042)
omar28744 0:cf5854b3296f 500 {
omar28744 0:cf5854b3296f 501 a2=0;
omar28744 0:cf5854b3296f 502 accel_var=0;
omar28744 0:cf5854b3296f 503 }
omar28744 0:cf5854b3296f 504 else
omar28744 0:cf5854b3296f 505 {
omar28744 0:cf5854b3296f 506 accel_var=0.001;
omar28744 0:cf5854b3296f 507 }
omar28744 0:cf5854b3296f 508 if((a3<=0.042)&&a3>=-0.042)
omar28744 0:cf5854b3296f 509 {
omar28744 0:cf5854b3296f 510 a3=0;
omar28744 0:cf5854b3296f 511 accel_var=0;
omar28744 0:cf5854b3296f 512 }
omar28744 0:cf5854b3296f 513 else
omar28744 0:cf5854b3296f 514 {
omar28744 0:cf5854b3296f 515 accel_var=0.001;
omar28744 0:cf5854b3296f 516 }
omar28744 0:cf5854b3296f 517 if((a4<=0.035)&&a4>=-0.035)
omar28744 0:cf5854b3296f 518 {
omar28744 0:cf5854b3296f 519 a4=0;
omar28744 0:cf5854b3296f 520 accel_var=0;
omar28744 0:cf5854b3296f 521 }
omar28744 0:cf5854b3296f 522 else
omar28744 0:cf5854b3296f 523 {
omar28744 0:cf5854b3296f 524 accel_var=0.001;
omar28744 0:cf5854b3296f 525 }
omar28744 0:cf5854b3296f 526 }
omar28744 0:cf5854b3296f 527
omar28744 0:cf5854b3296f 528 void compute_orientation()
omar28744 0:cf5854b3296f 529 {
omar28744 0:cf5854b3296f 530 roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2);
omar28744 0:cf5854b3296f 531 pitch = asinf(-2.0f * (q1*q3 - q0*q2));
omar28744 0:cf5854b3296f 532 yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
omar28744 0:cf5854b3296f 533
omar28744 0:cf5854b3296f 534
omar28744 0:cf5854b3296f 535 pitch *= 180.0f / PI;
omar28744 0:cf5854b3296f 536 yaw *= 180.0f / PI;
omar28744 0:cf5854b3296f 537 // yaw += 0.2f; // Magnetic declination at Kuching Malaysia
omar28744 0:cf5854b3296f 538 roll *= 180.0f / PI;
omar28744 0:cf5854b3296f 539 /*if(roll>0)
omar28744 0:cf5854b3296f 540 roll = -(180-roll);
omar28744 0:cf5854b3296f 541 else if(roll<0)
omar28744 0:cf5854b3296f 542 roll = 180+roll;*/
omar28744 0:cf5854b3296f 543 }
omar28744 0:cf5854b3296f 544
omar28744 0:cf5854b3296f 545 void IMU_GPS_Fusion()
omar28744 0:cf5854b3296f 546 {
omar28744 0:cf5854b3296f 547 dest_P_N=latitudetometers(destinate_lat);
omar28744 0:cf5854b3296f 548 dest_P_E=longitudetometers(destinate_lng);
omar28744 0:cf5854b3296f 549 if(GPS_FLAG == 0)
omar28744 0:cf5854b3296f 550 {
omar28744 0:cf5854b3296f 551 kalmanfilterpreditction(GPSPNO, GPSVNO,-a2,actual_pk1N,actual_pk2N,actual_pk3N,actual_pk4N,axzerocount);
omar28744 0:cf5854b3296f 552 kalmanfilterpreditction(GPSPEO, GPSVEO,a3,actual_pk1E,actual_pk2E,actual_pk3E,actual_pk4E,ayzerocount);
omar28744 0:cf5854b3296f 553 last_vel=now_vel;
omar28744 0:cf5854b3296f 554 }
omar28744 0:cf5854b3296f 555 if(GPS_FLAG == 1)
omar28744 0:cf5854b3296f 556 {
omar28744 0:cf5854b3296f 557 // pc.printf("vel = %f\n\r",(now_vel-last_vel)/1000000.0f);
omar28744 0:cf5854b3296f 558 // pc.printf("vel = %f\n\r",deltat);
omar28744 0:cf5854b3296f 559 now_vel = t.read_us();
omar28744 0:cf5854b3296f 560 GPSPNcurrent=latitudetometers(tgps.location.lat());
omar28744 0:cf5854b3296f 561 GPSPEcurrent = latitudetometers(tgps.location.lng());
omar28744 0:cf5854b3296f 562 //if(GPSVNO!=0)
omar28744 0:cf5854b3296f 563 GPSVN = (GPSPNcurrent-GPSPNintial)/((now_vel-last_vel)/1000000.0f);//velocity from GPS readings North
omar28744 0:cf5854b3296f 564 GPSVE = (GPSPEcurrent-GPSPEintial)/((now_vel-last_vel)/1000000.0f);
omar28744 0:cf5854b3296f 565 kalmanfilterupdate(GPSPNcurrent,GPSVN, GPSPNO, GPSVNO,actual_pk1N,actual_pk2N,actual_pk3N,actual_pk4N);
omar28744 0:cf5854b3296f 566 kalmanfilterupdate(GPSPEcurrent,GPSVE, GPSPEO, GPSVEO,actual_pk1E,actual_pk2E,actual_pk3E,actual_pk4E);
omar28744 0:cf5854b3296f 567 GPSPNintial = GPSPNcurrent;
omar28744 0:cf5854b3296f 568 GPSPEintial = GPSPEcurrent;
omar28744 0:cf5854b3296f 569 //printf ("GPS = %0.2f\n\r",sqrt((unposition_to_dest_N*unposition_to_dest_N)+(unposition_to_dest_E*unposition_to_dest_E)));
omar28744 0:cf5854b3296f 570 unposition_to_dest_N = dest_P_N - GPSPNcurrent;
omar28744 0:cf5854b3296f 571 unposition_to_dest_E = dest_P_E - GPSPEcurrent;
omar28744 0:cf5854b3296f 572 }
omar28744 0:cf5854b3296f 573 //North position
omar28744 0:cf5854b3296f 574 position_filt_N = GPSPNO-GpsstartingptN;
omar28744 0:cf5854b3296f 575 position_unfilt_N =GPSPNcurrent-GpsstartingptN;
omar28744 0:cf5854b3296f 576 // East position
omar28744 0:cf5854b3296f 577 position_filt_E = GPSPEO-GpsstartingptE;
omar28744 0:cf5854b3296f 578 position_unfilt_E = GPSPEcurrent-GpsstartingptE;
omar28744 0:cf5854b3296f 579 position_to_dest_N = dest_P_N-GPSPNO;
omar28744 0:cf5854b3296f 580 position_to_dest_E = dest_P_E-GPSPEO;
omar28744 0:cf5854b3296f 581
omar28744 0:cf5854b3296f 582 tgps.dist_to_coord(GPSPNO,GPSPEO, predicted_lat, predicted_lng);
omar28744 0:cf5854b3296f 583 dest = tgps.courseTo(predicted_lat,predicted_lng,destinate_lat,destinate_lng);
omar28744 0:cf5854b3296f 584
omar28744 0:cf5854b3296f 585 // pc.printf("%f, %f\n\r",predicted_lat, predicted_lng);
omar28744 0:cf5854b3296f 586 // pc.printf("%.2f\n\r",position_filt_N);//<------------------------worksN
omar28744 0:cf5854b3296f 587 //pc.printf("%.2f\n\r",position_filt_E);//<------------------------worksE
omar28744 0:cf5854b3296f 588
omar28744 0:cf5854b3296f 589 //pc.printf("%.4f & %.4f\n\r",position_filt_N,position_unfilt_N);
omar28744 0:cf5854b3296f 590 //important
omar28744 0:cf5854b3296f 591 //pc.printf("Roll = %.2f, Pitch = %.2f, Yaw = %.2f\n\r",roll,pitch,yaw);
omar28744 0:cf5854b3296f 592 // algo to choose the shortes angle to turn<<<<<<<<<<<<,---- important note
omar28744 0:cf5854b3296f 593 corrected_ang = yaw-dest;
omar28744 0:cf5854b3296f 594 if(corrected_ang>180)
omar28744 0:cf5854b3296f 595 corrected_ang = corrected_ang-360;
omar28744 0:cf5854b3296f 596 else if(corrected_ang<-180)
omar28744 0:cf5854b3296f 597 corrected_ang = corrected_ang+360;
omar28744 0:cf5854b3296f 598 corrected_ang = - corrected_ang; //convention C.C.W destination is positive angle
omar28744 0:cf5854b3296f 599 //pc.printf("Y = %.2f, TC = %.2f\n\r",yaw, corrected_ang);//<----------------- Heading
omar28744 0:cf5854b3296f 600 // pc.printf("ax = %.2f, ay = %.2f, az = %.2f\n\r",ax,ay,az);
omar28744 0:cf5854b3296f 601 //pc.printf("gx = %.4f, gy = %.4f, gz = %.4f\n\r",gx,gy,gz);
omar28744 0:cf5854b3296f 602 //pc.printf("mx = %.2f, my = %.2f, mz = %.2f\n\r",mx,my,mz);
omar28744 0:cf5854b3296f 603 //printf("%i\n\r",GPS_FLAG);
omar28744 0:cf5854b3296f 604 if(GPS_FLAG == 1)
omar28744 0:cf5854b3296f 605 {
omar28744 0:cf5854b3296f 606 a2 = 0; //Jerk limiter
omar28744 0:cf5854b3296f 607 a3 = 0; //Jerk limiter
omar28744 0:cf5854b3296f 608 printf ("GPS = %0.2f\n\r",sqrt((unposition_to_dest_N*unposition_to_dest_N)+(unposition_to_dest_E*unposition_to_dest_E)));
omar28744 0:cf5854b3296f 609 //printf("gps\n\r");
omar28744 0:cf5854b3296f 610 GPS_FLAG = 0;
omar28744 0:cf5854b3296f 611 //pc.printf("GPS%.2f\n\r",dest);
omar28744 0:cf5854b3296f 612 //pc.printf("GPS %f, %f\n\r",tgps.location.lat(), tgps.location.lng());
omar28744 0:cf5854b3296f 613 //pc.printf("GPS%.2f\n\r",position_unfilt_N);//<------------------------worksN
omar28744 0:cf5854b3296f 614 // pc.printf("G PS%.2f\n\r",dest);//<----------------Course
omar28744 0:cf5854b3296f 615 //pc.printf("GPS%.2f\n\r",position_unfilt_E);//<------------------------worksE
omar28744 0:cf5854b3296f 616 }
omar28744 0:cf5854b3296f 617
omar28744 0:cf5854b3296f 618 /* pc.printf("%f\n\r",(now_vel-last_vel)/1000000.0f);
omar28744 0:cf5854b3296f 619 pc.printf("%.2f\n\r",deltat);*/
omar28744 0:cf5854b3296f 620 //if(GPS_FLAG == 0)
omar28744 0:cf5854b3296f 621 //pc.printf("position filter = %f\n\r",l);
omar28744 0:cf5854b3296f 622
omar28744 0:cf5854b3296f 623 // __enable_irq();
omar28744 0:cf5854b3296f 624
omar28744 0:cf5854b3296f 625 // led = 0;
omar28744 0:cf5854b3296f 626
omar28744 0:cf5854b3296f 627 //led=1;
omar28744 0:cf5854b3296f 628 //pc.printf("vel = %f\n\r",(now_vel-last_vel)/1000000.0f);
omar28744 0:cf5854b3296f 629 // pc.printf("position un = %f\n\r",tgps.location.lat());
omar28744 0:cf5854b3296f 630
omar28744 0:cf5854b3296f 631
omar28744 0:cf5854b3296f 632 // algo used to reset the velocity to zero if no acceleration is detected for some time
omar28744 0:cf5854b3296f 633 //x-axis
omar28744 0:cf5854b3296f 634 if(a2==0)
omar28744 0:cf5854b3296f 635 axzerocount++;
omar28744 0:cf5854b3296f 636 else
omar28744 0:cf5854b3296f 637 axzerocount = 0;
omar28744 0:cf5854b3296f 638 /* if(axzerocount>=15)
omar28744 0:cf5854b3296f 639 {
omar28744 0:cf5854b3296f 640 vx = 0;
omar28744 0:cf5854b3296f 641 vxo = 0;
omar28744 0:cf5854b3296f 642 }
omar28744 0:cf5854b3296f 643 else
omar28744 0:cf5854b3296f 644 {
omar28744 0:cf5854b3296f 645 }*/
omar28744 0:cf5854b3296f 646 //y-axis
omar28744 0:cf5854b3296f 647 if(a3==0)
omar28744 0:cf5854b3296f 648 ayzerocount++;
omar28744 0:cf5854b3296f 649 else
omar28744 0:cf5854b3296f 650 ayzerocount = 0;
omar28744 0:cf5854b3296f 651
omar28744 0:cf5854b3296f 652 /* if(ayzerocount>=15)
omar28744 0:cf5854b3296f 653 {
omar28744 0:cf5854b3296f 654 vy = 0;
omar28744 0:cf5854b3296f 655 vyo= 0;
omar28744 0:cf5854b3296f 656 }
omar28744 0:cf5854b3296f 657 //z-axis
omar28744 0:cf5854b3296f 658 if(a4==0)
omar28744 0:cf5854b3296f 659 azzerocount++;
omar28744 0:cf5854b3296f 660 else
omar28744 0:cf5854b3296f 661 azzerocount = 0;
omar28744 0:cf5854b3296f 662
omar28744 0:cf5854b3296f 663 if(azzerocount>=15)
omar28744 0:cf5854b3296f 664 {
omar28744 0:cf5854b3296f 665 vz = 0;
omar28744 0:cf5854b3296f 666 vzo= 0;
omar28744 0:cf5854b3296f 667 }
omar28744 0:cf5854b3296f 668 // end of reset algo */
omar28744 0:cf5854b3296f 669 }