Darrien Galid
Self-navigating boat program with sensors and control system fused
Diff: main.cpp
- Revision:
- 0:cf5854b3296f
- Child:
- 1:736ae4695570
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri May 10 08:45:41 2019 +0000
@@ -0,0 +1,669 @@
+#include "mbed.h"
+#include "MPU9250.h"
+#include "math.h"
+#include "TinyGPSplus.h"
+
+#define TX5 PTD3
+#define RX5 PTD2
+#define GPSBaud 9600
+#define destinate_lat 1.533166
+#define destinate_lng 110.357162
+
+DigitalOut led(PTD3);
+ MPU9250 mpu9250; // Instantiate MPU9250 class
+
+ Timer t; // setup a timer
+
+ Serial pc(USBTX, USBRX); // Serial communication to display resuslts on PC
+ Serial GPSSerial(TX5, RX5); //Serial communication to read data from GPS
+ TinyGPSPlus tgps; // Instantiate TinyGPS class
+
+
+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;
+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;
+char buffer[14];
+uint8_t whoami = 0;
+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;
+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;
+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;
+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;
+
+void setup(); // used in calibration of MPU9250 and setting baud rates for serial com.
+
+void Read_MPU9250_Raw();// used in reading raw accel, gyro, magneto in 2 axis from the MPU9250 (IMU)
+
+// used to fuse the accelerometer with gyro and magnetometer readings to determine the orientation in quaternion form
+void MadgwickUpdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float deltat);
+
+void compute_accel_world_ref();//Used to rotate the acceleration from the sensor fram to the world frame (NED) using quaternion
+
+void compute_orientation();// used in computing the roll, Pitch and yaw from quaternion
+
+void SerialInterruptHandler(void);// Interupt function where GPS data is read
+
+double latitudetometers(double latitudetom);// convert long to meters (North) on Earth
+
+double longitudetometers(double longitudetom);// convert long to meters (East) on Earth
+
+// used to perform deadreckoning
+void kalmanfilterpreditction(double &PO, double &VO, double acceleration, double &actual_pk1, double &actual_pk2, double &actual_pk3, double &actual_pk4,int azerocount);
+
+// used to update the accumilative (IMU) readings with absolute GPS readings
+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);
+
+void IMU_GPS_Fusion();// used in impleminting the Kalman filter to output predicted velocity and position.
+
+float invSqrt(float x);// inverse square root function
+
+
+int main()
+{
+
+ setup();// code to setup GPS/MPU9250
+
+ /* Main Loop*/
+ while(1)
+ {
+ Read_MPU9250_Raw();
+ MadgwickUpdate(gx, gy, gz, ax, ay, az, mx, my, mz, deltat);
+ compute_accel_world_ref();
+ compute_orientation();
+
+ // GPSSerial.attach(&SerialInterruptHandler, Serial::RxIrq);// Interrupt trigerrer
+ //if(t.read_ms()/1000>(start_time-2))
+ //stop_avg=1;
+ 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
+ {
+
+ GPSSerial.attach(&SerialInterruptHandler, Serial::RxIrq);// Interrupt trigerrer
+ if(ready == 1)
+ {
+ IMU_GPS_Fusion();
+
+ //**************************************
+ //a2 //This variable is acceleration North
+ //a3 //This variable is acceleration East
+ //yaw //This variable is the yaw (Heading)
+ //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)
+ //position_to_dest_N//This variable returns distance needed to reach the destination in North (meters).
+ //position_to_dest_E//This variable returns distance needed to reach the destination in East.
+ //position_filt_N //This variable returns how much you have moved in North (meters)
+ //position_filt_E //This variable return how much you have moved in East (meters)
+ //GPSVNO // This variable returns your velocity in North
+ //GPSEN0 //This variable returns your velocity in East
+ //*****************************************
+
+ //a4 //This variable is acceleration Down
+ //roll //This variable is the roll
+ //Pitch // This variable is the Pitch
+ //position_filt_N //This variable returns how much you have moved in North
+ //position_filt_E //This variable return how much you have moved in East
+ //GPSPNO //This variable returns your current position in the world in term of meters (North)
+ //GPSPEO //This variable returns your current position in the world in term of meters (East
+ //GpsstartingptN; //This variable returns your starting North position in terms of meters (Measured from latitude of GPS)
+ //GpsstartingptE; //This variable returns your starting position East in terms of meters (Measured from latitude of GPS)
+
+ if (print_count ==6)
+ {
+ 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)));
+ //printf ("Fd = %.2f\n\r",sqrt((position_to_dest_N*position_to_dest_N)+(position_to_dest_E*position_to_dest_E)));
+ print_count = 0;
+ }
+ print_count++;
+ //printf("%i\n\r",count_gps_reads);
+ }
+
+ }
+
+ }
+
+}
+
+void MadgwickUpdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float deltat)
+{
+ float recipNorm;
+ float s0, s1, s2, s3;
+ float qDot1, qDot2, qDot3, qDot4;
+ float hx, hy;
+ 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;
+
+ // Rate of change of quaternion from gyroscope
+ qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
+ qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
+ qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
+ qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
+
+ // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
+ if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+
+ // Normalise accelerometer measurement
+ recipNorm = invSqrt(ax * ax + ay * ay + az * az);
+ ax *= recipNorm;
+ ay *= recipNorm;
+ az *= recipNorm;
+
+ // Normalise magnetometer measurement
+ recipNorm = invSqrt(mx * mx + my * my + mz * mz);
+ mx *= recipNorm;
+ my *= recipNorm;
+ mz *= recipNorm;
+
+ // Auxiliary variables to avoid repeated arithmetic
+ _2q0mx = 2.0f * q0 * mx;
+ _2q0my = 2.0f * q0 * my;
+ _2q0mz = 2.0f * q0 * mz;
+ _2q1mx = 2.0f * q1 * mx;
+ _2q0 = 2.0f * q0;
+ _2q1 = 2.0f * q1;
+ _2q2 = 2.0f * q2;
+ _2q3 = 2.0f * q3;
+ _2q0q2 = 2.0f * q0 * q2;
+ _2q2q3 = 2.0f * q2 * q3;
+ q0q0 = q0 * q0;
+ q0q1 = q0 * q1;
+ q0q2 = q0 * q2;
+ q0q3 = q0 * q3;
+ q1q1 = q1 * q1;
+ q1q2 = q1 * q2;
+ q1q3 = q1 * q3;
+ q2q2 = q2 * q2;
+ q2q3 = q2 * q3;
+ q3q3 = q3 * q3;
+
+ // Reference direction of Earth's magnetic field
+ hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
+ hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
+ _2bx = sqrtf(hx * hx + hy * hy);
+ _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
+ _4bx = 2.0f * _2bx;
+ _4bz = 2.0f * _2bz;
+
+ // Gradient decent algorithm corrective step
+ 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);
+ 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);
+ 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);
+ 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);
+ recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
+ s0 *= recipNorm;
+ s1 *= recipNorm;
+ s2 *= recipNorm;
+ s3 *= recipNorm;
+
+ // Apply feedback step
+ qDot1 -= beta * s0;
+ qDot2 -= beta * s1;
+ qDot3 -= beta * s2;
+ qDot4 -= beta * s3;
+ }
+
+ // Integrate rate of change of quaternion to yield quaternion
+ q0 += qDot1 * deltat;
+ q1 += qDot2 * deltat;
+ q2 += qDot3 * deltat;
+ q3 += qDot4 * deltat;
+
+ // Normalise quaternion
+ recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+ q0 *= recipNorm;
+ q1 *= recipNorm;
+ q2 *= recipNorm;
+ q3 *= recipNorm;
+
+}
+float invSqrt(float x)
+{
+ float halfx = 0.5f * x;
+ float y = x;
+ long i = *(long*)&y;
+ i = 0x5f3759df - (i>>1);
+ y = *(float*)&i;
+ y = y * (1.5f - (halfx * y * y));
+ y = y * (1.5f - (halfx * y * y));
+ return y;
+}
+double latitudetometers(double latitudetom)// Position in North
+{
+ double lattom;
+ lattom = tgps.distanceBetween(latitudetom,0, 0, 0);
+ if(latitudetom<0)
+ lattom *=-1;
+ return lattom;
+}
+double longitudetometers(double longitudetom)// Position in East
+{
+ double lngtom;
+ lngtom = tgps.distanceBetween(0,longitudetom, 0, 0);
+ if(longitudetom<0)
+ lngtom *=-1;
+ return lngtom;
+}
+
+void SerialInterruptHandler(void)
+{
+ if(GPSSerial.readable())
+ {
+
+ if (tgps.encode(GPSSerial.getc()))
+ { gps_lat = tgps.location.lat();
+ gps_long = tgps.location.lng();
+ if((gps_lat-gps_lato !=0) || (gps_long-gps_longo !=0))
+ GPS_FLAG = 1;
+ //else
+ //{
+ //GPS_FLAG = 0;
+ // pc.printf("position filter = %f\n\r",l);
+ // }
+ gps_lato=gps_lat;
+ gps_longo=gps_long;
+ if(GPSPNO==0) //loop one time only
+ {
+ GPSPNO=latitudetometers(tgps.location.lat());
+ GPSPEO=longitudetometers(tgps.location.lng());
+ GPSVEO=0;
+ GPSVNO=0;
+ GPSPNintial =GPSPNO;// used to measure the velocity by comparing the distance moved every 1 sec
+ GPSPEintial =GPSPEO;
+ GpsstartingptN =GPSPNO;// starting point refrence
+ GpsstartingptE = GPSPEO;
+
+ }
+ else
+ ready = 1;
+ // if(ready ==1 &&GPS_FLAG == 1 && stop_avg== 0)
+ //{
+ // sum_avg_lat = sum_avg_lat+GPSPNO;
+ // sum_avg_lng = sum_avg_lng+GPSPEO;
+
+ //GPSPNO=latitudetometers(tgps.location.lat());
+ //GPSPEO=latitudetometers(tgps.location.lat());
+ //count_gps_reads++;
+ //}
+ //if(stop_avg ==1&&onetime==0)
+ //{
+ // GPSPNO=sum_avg_lat/count_gps_reads;
+ //GPSPEO=sum_avg_lng/count_gps_reads;
+ //onetime=1;
+ //}
+
+ }
+ }
+ return;
+}
+void kalmanfilterpreditction(double &PO, double &VO, double acceleration, double &actual_pk1, double &actual_pk2, double &actual_pk3, double &actual_pk4, int azerocount)
+{
+ double predicted_pk1,predicted_pk2,predicted_pk3,predicted_pk4,Predicted_p,Predicted_v;
+ Predicted_p = PO+(VO*deltat)+(0.5*deltat*deltat*acceleration);
+ Predicted_v = VO+acceleration*deltat;
+ predicted_pk1 = actual_pk1 +(deltat*actual_pk2)+(deltat*actual_pk3)+(deltat*deltat*actual_pk4)+accel_var;
+ predicted_pk2 = actual_pk2 + (deltat*actual_pk4);
+ predicted_pk3 = actual_pk3 + (deltat*actual_pk4);
+ predicted_pk4 = actual_pk4 + accel_var;
+ if(azerocount>=5)
+ {
+ VO= 0;
+ }
+ else
+ {
+ VO = Predicted_v;
+ }
+ PO = Predicted_p;
+ actual_pk1 = predicted_pk1;
+ actual_pk2 = predicted_pk2;
+ actual_pk3 = predicted_pk3;
+ actual_pk4 = predicted_pk4;
+}
+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)
+{
+ double y = 1/((actual_pk1*actual_pk4+actual_pk1*GPS_VAR+actual_pk4*GPS_VAR+GPS_VAR*GPS_VAR)-(actual_pk2*actual_pk3));
+ double GK1 = (actual_pk1*actual_pk4+actual_pk1*GPS_VAR-actual_pk2*actual_pk3)*y;
+ double GK2 = (actual_pk2*GPS_VAR)*y;
+ double GK3 = (actual_pk3*GPS_VAR)*y;
+ double GK4 = (actual_pk1*actual_pk4+actual_pk4*GPS_VAR-actual_pk2*actual_pk3)*y;
+ if(predicted_vel!=0)
+ {
+ predicted_pos = predicted_pos+GK1*GPS_POS-GK1*predicted_pos+GK3*GPS_VEL-GK3*predicted_vel;
+ predicted_vel = predicted_vel+GK2*GPS_POS-GK2*predicted_pos+GK4*GPS_VEL-GK4*predicted_vel;
+ }
+ // predicted_pos = predicted_pos+GK1*GPS_POS-GK1*predicted_pos;
+ // predicted_vel = predicted_vel+GK2*GPS_POS-GK2*predicted_pos;
+ actual_pk1 = actual_pk1-actual_pk1*GK1-GK3*actual_pk2;
+ actual_pk2 = actual_pk2-actual_pk2*GK4-GK2*actual_pk1;
+ actual_pk3 = actual_pk3-actual_pk3*GK1-GK3*actual_pk4;
+ actual_pk4 = actual_pk4-actual_pk4*GK4-GK2*actual_pk3;
+}
+
+void setup()
+{
+ GPSSerial.baud(GPSBaud);
+ wait(0.001);
+ pc.baud(9600);
+ myled = 0; // turn off led
+
+ wait(5);
+
+ //Set up I2C
+ i2c.frequency(400000); // use fast (400 kHz) I2C
+
+ t.start(); // enable system timer
+
+ myled = 1; // turn on led
+
+ // Read the WHO_AM_I register, this is a good test of communication
+ whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
+ pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x73\n\r");
+ myled = 1;
+
+ if (whoami == 0x73) // WHO_AM_I should always be 0x73
+ {
+ pc.printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami);
+ pc.printf("MPU9250 is online...\n\r");
+ wait(1);
+
+ mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
+
+ mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
+ pc.printf("x-axis self test: acceleration trim within: %f pct of factory value\n\r", SelfTest[1]);
+ pc.printf("y-axis self test: acceleration trim within: %f pct of factory value\n\r", SelfTest[2]);
+ pc.printf("z-axis self test: acceleration trim within: %f pct of factory value\n\r", SelfTest[2]);
+ pc.printf("x-axis self test: gyration trim within: %f pct of factory value\n\r", SelfTest[3]);
+ pc.printf("y-axis self test: gyration trim within: %f pct of factory value\n\r", SelfTest[4]);
+ pc.printf("z-axis self test: gyration trim within: %f pct of factory value\n\r", SelfTest[5]);
+
+ mpu9250.getAres(); // Get accelerometer sensitivity
+ mpu9250.getGres(); // Get gyro sensitivity
+ mpu9250.getMres(); // Get magnetometer sensitivity
+ pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
+ pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
+ pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
+
+ mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
+ pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
+ pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
+ pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
+ pc.printf("x accel bias = %f\n\r", accelBias[1]);
+ pc.printf("y accel bias = %f\n\r", accelBias[0]);
+ pc.printf("z accel bias = %f\n\r", accelBias[2]);
+ wait(2);
+
+ mpu9250.initMPU9250();
+ pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
+ wait(1);
+
+ mpu9250.initAK8963(magCalibration);
+ pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
+ pc.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale));
+ pc.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale));
+ if(Mscale == 0) pc.printf("Magnetometer resolution = 14 bits\n\r");
+ if(Mscale == 1) pc.printf("Magnetometer resolution = 16 bits\n\r");
+ if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
+ if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
+
+ pc.printf("Mag Calibration: Wave device in a figure eight until done!");
+ wait(4);
+ mpu9250.magcalMPU9250(magBias, magScale);
+ pc.printf("Mag Calibration done!\n\r");
+ pc.printf("x mag bias = %f\n\r", magBias[0]);
+ pc.printf("y mag bias = %f\n\r", magBias[1]);
+ pc.printf("z mag bias = %f\n\r", magBias[2]);
+ //predicted_pk = A*actual_pk;
+ //predicted_pk.print();
+ wait(2);
+ }
+
+ else
+
+ {
+ pc.printf("Could not connect to MPU9250: \n\r");
+ pc.printf("%#x \n", whoami);
+ myled = 0;
+
+ while(1) ; // Loop forever if communication doesn't happen
+ }
+}
+
+void Read_MPU9250_Raw()
+{
+ //while(movwind<64)
+ //{
+ mpu9250.readMPU9250Data(MPU9250Data); // INT cleared on any read
+ ax = (float)MPU9250Data[1]*aRes - accelBias[1]; // get actual g value, this depends on scale being set
+ ay = (float)MPU9250Data[0]*aRes - accelBias[0];
+ az = (float)MPU9250Data[2]*aRes - accelBias[2];
+ ax = (ax)*9.80665f;// to get it in m/s^2
+ ay = (ay)*9.80665f;// to get it in m/s^2
+ az = (-az)*9.80665f;// to get it in m/s^2
+ axf=axf+ax;
+ ayf=ayf+ay;
+ azf=azf+az;
+ movwind++;
+ //}
+ axf = axf/movwind;
+ ayf=ayf/movwind;
+ azf=azf/movwind;
+ movwind = 0;
+ //pc.printf("ax = %f", axf);
+ // pc.printf(" ay = %f",ayf);
+ //pc.printf(" az = %f m/s/s\n\r",azf);
+ mpu9250.readMagData(magCount); // Read the x/y/z adc values
+ // Calculate the magnetometer values in milliGauss
+ // Include factory calibration per data sheet and user environmental corrections
+ gx = (float)MPU9250Data[5]*gRes - gyroBias[1]; // get actual gyro value, this depends on scale being set
+ gy = (float)MPU9250Data[4]*gRes - gyroBias[0];
+ gz = (float)MPU9250Data[6]*gRes - gyroBias[2];
+
+ mx = (float)magCount[0]*mRes*magCalibration[0] - magBias[0];
+ my = (float)magCount[1]*mRes*magCalibration[1] - magBias[1];
+ mz = (float)magCount[2]*mRes*magCalibration[2] - magBias[2];
+
+
+ // poor man's soft iron calibration
+ mx *= magScale[0];
+ my *= magScale[1];
+ mz *= magScale[2];
+ Now = t.read_us();
+ // Calculate the gyro value into actual degrees per second
+ deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
+ lastUpdate = Now;
+
+ // Pass gyro rate as rad/s
+ mx = mx/10.0f;
+ my = my/10.0f;
+ mz = mz/10.0f;
+ gx = gx*(3.141592f/180.0f);
+ gy = gy*(3.141592f/180.0f);
+ gz = -gz*(3.141592f/180.0f);
+}
+
+void compute_accel_world_ref()
+{
+ //quaternion inverse calculation
+ qnorm = q0*q0+q1*q1+q2*q2+q3*q3;
+ q1i = q0/qnorm;
+ q2i = -q1/qnorm;
+ q3i = -q2/qnorm;
+ q4i = -q3/qnorm;
+ //quaternion rotation * acceleration vector
+ p1 = q0*0-q1*ax-q2*ay-q3*az;
+ p2 = q0*ax+q1*0+q2*az-q3*ay;
+ p3 = q0*ay-q1*az+q2*0+q3*ax;
+ p4 = q0*az+q1*ay-q2*ax+q3*0;
+ // compute acceleration vector
+ a1 = p1*q1i-p2*q2i-p3*q3i-p4*q4i; // = 0
+ a2 = p1*q2i+p2*q1i+p3*q4i-p4*q3i;
+ a3 = p1*q3i-p2*q4i+p3*q1i+p4*q2i;
+ a4 = p1*q4i+p2*q3i-p3*q2i+p4*q1i;
+ a4 = a4-9.80665f;//subtract the gravity component
+ //Filtered accel
+
+ // filter used to eliminate micromovements from filter readings
+ if((a2<=0.042)&&a2>=-0.042)
+ {
+ a2=0;
+ accel_var=0;
+ }
+ else
+ {
+ accel_var=0.001;
+ }
+ if((a3<=0.042)&&a3>=-0.042)
+ {
+ a3=0;
+ accel_var=0;
+ }
+ else
+ {
+ accel_var=0.001;
+ }
+ if((a4<=0.035)&&a4>=-0.035)
+ {
+ a4=0;
+ accel_var=0;
+ }
+ else
+ {
+ accel_var=0.001;
+ }
+}
+
+void compute_orientation()
+{
+ roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2);
+ pitch = asinf(-2.0f * (q1*q3 - q0*q2));
+ yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
+
+
+ pitch *= 180.0f / PI;
+ yaw *= 180.0f / PI;
+ // yaw += 0.2f; // Magnetic declination at Kuching Malaysia
+ roll *= 180.0f / PI;
+ /*if(roll>0)
+ roll = -(180-roll);
+ else if(roll<0)
+ roll = 180+roll;*/
+}
+
+void IMU_GPS_Fusion()
+{
+ dest_P_N=latitudetometers(destinate_lat);
+ dest_P_E=longitudetometers(destinate_lng);
+ if(GPS_FLAG == 0)
+ {
+ kalmanfilterpreditction(GPSPNO, GPSVNO,-a2,actual_pk1N,actual_pk2N,actual_pk3N,actual_pk4N,axzerocount);
+ kalmanfilterpreditction(GPSPEO, GPSVEO,a3,actual_pk1E,actual_pk2E,actual_pk3E,actual_pk4E,ayzerocount);
+ last_vel=now_vel;
+ }
+ if(GPS_FLAG == 1)
+ {
+ // pc.printf("vel = %f\n\r",(now_vel-last_vel)/1000000.0f);
+ // pc.printf("vel = %f\n\r",deltat);
+ now_vel = t.read_us();
+ GPSPNcurrent=latitudetometers(tgps.location.lat());
+ GPSPEcurrent = latitudetometers(tgps.location.lng());
+ //if(GPSVNO!=0)
+ GPSVN = (GPSPNcurrent-GPSPNintial)/((now_vel-last_vel)/1000000.0f);//velocity from GPS readings North
+ GPSVE = (GPSPEcurrent-GPSPEintial)/((now_vel-last_vel)/1000000.0f);
+ kalmanfilterupdate(GPSPNcurrent,GPSVN, GPSPNO, GPSVNO,actual_pk1N,actual_pk2N,actual_pk3N,actual_pk4N);
+ kalmanfilterupdate(GPSPEcurrent,GPSVE, GPSPEO, GPSVEO,actual_pk1E,actual_pk2E,actual_pk3E,actual_pk4E);
+ GPSPNintial = GPSPNcurrent;
+ GPSPEintial = GPSPEcurrent;
+ //printf ("GPS = %0.2f\n\r",sqrt((unposition_to_dest_N*unposition_to_dest_N)+(unposition_to_dest_E*unposition_to_dest_E)));
+ unposition_to_dest_N = dest_P_N - GPSPNcurrent;
+ unposition_to_dest_E = dest_P_E - GPSPEcurrent;
+ }
+ //North position
+ position_filt_N = GPSPNO-GpsstartingptN;
+ position_unfilt_N =GPSPNcurrent-GpsstartingptN;
+ // East position
+ position_filt_E = GPSPEO-GpsstartingptE;
+ position_unfilt_E = GPSPEcurrent-GpsstartingptE;
+ position_to_dest_N = dest_P_N-GPSPNO;
+ position_to_dest_E = dest_P_E-GPSPEO;
+
+ tgps.dist_to_coord(GPSPNO,GPSPEO, predicted_lat, predicted_lng);
+ dest = tgps.courseTo(predicted_lat,predicted_lng,destinate_lat,destinate_lng);
+
+ // pc.printf("%f, %f\n\r",predicted_lat, predicted_lng);
+ // pc.printf("%.2f\n\r",position_filt_N);//<------------------------worksN
+ //pc.printf("%.2f\n\r",position_filt_E);//<------------------------worksE
+
+ //pc.printf("%.4f & %.4f\n\r",position_filt_N,position_unfilt_N);
+ //important
+ //pc.printf("Roll = %.2f, Pitch = %.2f, Yaw = %.2f\n\r",roll,pitch,yaw);
+ // algo to choose the shortes angle to turn<<<<<<<<<<<<,---- important note
+ corrected_ang = yaw-dest;
+ if(corrected_ang>180)
+ corrected_ang = corrected_ang-360;
+ else if(corrected_ang<-180)
+ corrected_ang = corrected_ang+360;
+ corrected_ang = - corrected_ang; //convention C.C.W destination is positive angle
+ //pc.printf("Y = %.2f, TC = %.2f\n\r",yaw, corrected_ang);//<----------------- Heading
+ // pc.printf("ax = %.2f, ay = %.2f, az = %.2f\n\r",ax,ay,az);
+ //pc.printf("gx = %.4f, gy = %.4f, gz = %.4f\n\r",gx,gy,gz);
+ //pc.printf("mx = %.2f, my = %.2f, mz = %.2f\n\r",mx,my,mz);
+ //printf("%i\n\r",GPS_FLAG);
+ if(GPS_FLAG == 1)
+ {
+ a2 = 0; //Jerk limiter
+ a3 = 0; //Jerk limiter
+ printf ("GPS = %0.2f\n\r",sqrt((unposition_to_dest_N*unposition_to_dest_N)+(unposition_to_dest_E*unposition_to_dest_E)));
+ //printf("gps\n\r");
+ GPS_FLAG = 0;
+ //pc.printf("GPS%.2f\n\r",dest);
+ //pc.printf("GPS %f, %f\n\r",tgps.location.lat(), tgps.location.lng());
+ //pc.printf("GPS%.2f\n\r",position_unfilt_N);//<------------------------worksN
+ // pc.printf("G PS%.2f\n\r",dest);//<----------------Course
+ //pc.printf("GPS%.2f\n\r",position_unfilt_E);//<------------------------worksE
+ }
+
+ /* pc.printf("%f\n\r",(now_vel-last_vel)/1000000.0f);
+ pc.printf("%.2f\n\r",deltat);*/
+ //if(GPS_FLAG == 0)
+ //pc.printf("position filter = %f\n\r",l);
+
+ // __enable_irq();
+
+ // led = 0;
+
+ //led=1;
+ //pc.printf("vel = %f\n\r",(now_vel-last_vel)/1000000.0f);
+ // pc.printf("position un = %f\n\r",tgps.location.lat());
+
+
+ // algo used to reset the velocity to zero if no acceleration is detected for some time
+ //x-axis
+ if(a2==0)
+ axzerocount++;
+ else
+ axzerocount = 0;
+ /* if(axzerocount>=15)
+ {
+ vx = 0;
+ vxo = 0;
+ }
+ else
+ {
+ }*/
+ //y-axis
+ if(a3==0)
+ ayzerocount++;
+ else
+ ayzerocount = 0;
+
+ /* if(ayzerocount>=15)
+ {
+ vy = 0;
+ vyo= 0;
+ }
+ //z-axis
+ if(a4==0)
+ azzerocount++;
+ else
+ azzerocount = 0;
+
+ if(azzerocount>=15)
+ {
+ vz = 0;
+ vzo= 0;
+ }
+ // end of reset algo */
+}
\ No newline at end of file