File content as of revision 1:736ae4695570:

#include "mbed.h"
#include "MPU9250.h"
#include "math.h"
#include "TinyGPSplus.h"
#include "Servo.h"
#include "PID.h"

Servo motor_servo(PTB1); // Boat throttle servo PWM
Servo rudder_servo(PTB0); // Boat rudder servo PWM
DigitalOut led(PTD3);
MPU9250 mpu9250;  // Instantiate MPU9250 class
Timer t; // setup a timer
Serial device(PTE0, PTE1); //RF Device tx and rx at PTE0 and PTE1
Serial pc(USBTX, USBRX); // Serial communication to display resuslts on PC
TinyGPSPlus tgps; // Instantiate TinyGPS class
PID velocity_controller(5.0, 1.0, 0.4, 0.1);
PID heading_controller(5.0, 2.0, 0.4, 0.1);

#define TX5 PTD3
#define RX5 PTD2
#define GPSBaud 9600
#define destinate_lat 1.532277
#define destinate_lng 110.357754

Serial GPSSerial(TX5, RX5); //Serial communication to read data from GPS
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.0023,accel_varb=0.0023,GPS_VAR = 500;
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 ManualOverride(); // Switch the boat into manual control mode
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

class USV {
    private:
 
    public:
        float u; // Surge velocity
        float v; // Sway velocity
        float r; // Yaw velocity
        float x; // Position in East
        float y; // Position in North
        float psi; // Yaw angle
};

class Setpoint {
    private:
    
    public:
        float vel;
        float north;
        float east;
        float psi;     
};        

int main()
{
    // 
    motor_servo = 0.0;
    wait(0.5);
    motor_servo = 1.0;
    wait(1.0);
    motor_servo = 0.0;
    rudder_servo = 0.5;
    float motor_out = 0.0;
    float rudder_out = 0.0;
    float dist = 0.0;
    char command;
    bool start_flag = false;
    bool destination_flag = false;
    int read_count = 0;
    USV boat;
    Setpoint sp;
    device.baud(9600);
    device.printf("Wireless UART connection initialised.\n\r");

    // Sensor setup
    device.printf("Press 'r' to calibrate sensors.\n\r");
    while (!start_flag) {
        if (device.readable() != 0) command = device.getc();
        if (command == 'r') start_flag = true;
    }

    device.printf("Calibrate command accepted.\n\r");  
    command = ' ';
    start_flag = false;
    setup();// code to setup GPS/MPU9250

    while(t.read_ms()/1000<start_time) {
        Read_MPU9250_Raw();
        MadgwickUpdate(gx, gy, gz, ax, ay, az, mx, my, mz, deltat);
        compute_accel_world_ref();
        compute_orientation();
    }
    device.printf("Sensor calibration done.\n\r");

    // Wait for start command
    device.printf("Press 'r' to set setpoint and start.\n\r");
    while (!start_flag) {
        if (device.readable() != 0) command = device.getc();
        if (command == 'r') start_flag = true;
    }

    device.printf("Start command accepted.\n\r");  
    command = ' ';
    start_flag = false;
  
    // Declare setpoints
    sp.vel = 1.0;
    sp.north = destinate_lat;
    sp.east = destinate_lng;
    device.printf("Velocity setpoint is %f.\n\r", sp.vel);
    device.printf("North setpoint is %f.\n\r", sp.north);
    device.printf("East setpoint is %f.\n\r", sp.east);

    velocity_controller.setOutputLimits(0.0, 0.12);
    velocity_controller.setMode(AUTO_MODE);
    velocity_controller.setSetPoint(sp.vel);
    device.printf("Velocity controller initialised.\n\r"); 
    
    heading_controller.setOutputLimits(-0.3, 0.3);
    heading_controller.setMode(AUTO_MODE);
    device.printf("Heading controller initialised.\n\r");

    device.printf("Moving boat to destination.\n\r");

    while(!destination_flag) {
        // Update boat states from sensor
        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 (ready == 1) {

            IMU_GPS_Fusion();
            boat.psi = yaw*DEG_TO_RAD;
            boat.u = sqrt((GPSVNO*GPSVNO) + (GPSVEO*GPSVEO));
            boat.x = GPSPEO;
            boat.y = GPSPNO;    

            // Update heading setpoint
            sp.psi = dest*DEG_TO_RAD;
            heading_controller.setSetPoint(sp.psi); 
          
            //Update the process variable.
            velocity_controller.setProcessValue(boat.u);
            heading_controller.setProcessValue(boat.psi);
          
            //Set the new output.
            motor_out = velocity_controller.compute();
            rudder_out = heading_controller.compute();
            dist = sqrt((position_to_dest_N*position_to_dest_N)+(position_to_dest_E*position_to_dest_E));
            motor_servo = motor_out;
            rudder_servo = 0.5 + rudder_out;
            //device.printf("Error = %f, Rudder output = %f\n\rRudder angle = %f\n\r", corrected_ang, rudder_out, 0.5+rudder_out);
            //device.printf("Motor_out = %f\n\r",motor_out);
      
            // Check if destination reached
            if(dist < 5.0) destination_flag = true;

            // Check if manual override commanded
            if (device.readable() != 0) command = device.getc();
            if (command == 'r') ManualOverride();
      
            if(read_count == 10) {
                device.printf("Dist = %f\n\r", dist);
                read_count = 0;
            }
      
            read_count++;
            //wait(0.1);
        }
    }
    
    device.printf("Destination reached. Switching to manual mode.\n\r");
    ManualOverride();

    return 0;
}

void ManualOverride() {
    device.printf("Manual mode entered.\n\r");
    char command;
    motor_servo = 0.0;
    rudder_servo = 0.5;
    device.printf("Ready to accept commands.\n\r");

    while(1) {       
        while (device.readable() != 0) {
                
            command = device.getc();

            switch (command){
                case 'w':
                    if (motor_servo < 1.0) motor_servo = motor_servo + 0.01;
                    device.printf("Motor speed increased\n\r");
                    break;
                        
                case 's':
                    if (motor_servo > 0.0) motor_servo = motor_servo - 0.01;
                    device.printf("Motor speed decreased\n\r");
                    break;
                        
                case 'a':
                    if (rudder_servo > 0.3) rudder_servo = rudder_servo - 0.1;
                    device.printf("Rudder tilted right\n\r");
                    break;
                        
                case 'd':
                    if (rudder_servo < 0.8) rudder_servo = rudder_servo + 0.1;
                    device.printf("Rudder tilted left\n\r");
                    break;
                        
                case 'r':
                    motor_servo = 0.0;
                    rudder_servo = 0.5;
                    device.printf("Reset\n\r");
                    break;
            }  
        }                            
    }
}

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
  device.printf("I AM 0x%x\n\r", whoami); device.printf("I SHOULD BE 0x73\n\r");
  myled = 1;
  
  if (whoami == 0x73) // WHO_AM_I should always be 0x73
  {  
    device.printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami);
    device.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
    device.printf("x-axis self test: acceleration trim within: %f pct of factory value\n\r", SelfTest[1]);  
    device.printf("y-axis self test: acceleration trim within: %f pct of factory value\n\r", SelfTest[2]);  
    device.printf("z-axis self test: acceleration trim within: %f pct of factory value\n\r", SelfTest[2]);  
    device.printf("x-axis self test: gyration trim within: %f pct of factory value\n\r", SelfTest[3]);  
    device.printf("y-axis self test: gyration trim within: %f pct of factory value\n\r", SelfTest[4]);  
    device.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
    device.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
    device.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
    device.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  
    device.printf("x gyro bias = %f\n\r", gyroBias[0]);
    device.printf("y gyro bias = %f\n\r", gyroBias[1]);
    device.printf("z gyro bias = %f\n\r", gyroBias[2]);
    device.printf("x accel bias = %f\n\r", accelBias[1]);
    device.printf("y accel bias = %f\n\r", accelBias[0]);
    device.printf("z accel bias = %f\n\r", accelBias[2]);
    wait(2);

    mpu9250.initMPU9250(); 
    device.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);
    device.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
    device.printf("Accelerometer full-scale range = %f  g\n\r", 2.0f*(float)(1<<Ascale));
    device.printf("Gyroscope full-scale range = %f  deg/s\n\r", 250.0f*(float)(1<<Gscale));
    if(Mscale == 0) device.printf("Magnetometer resolution = 14  bits\n\r");
    if(Mscale == 1) device.printf("Magnetometer resolution = 16  bits\n\r");
    if(Mmode == 2) device.printf("Magnetometer ODR = 8 Hz\n\r");
    if(Mmode == 6) device.printf("Magnetometer ODR = 100 Hz\n\r");

    device.printf("Mag Calibration: Wave device in a figure eight until done!");
    wait(4);
    mpu9250.magcalMPU9250(magBias, magScale);
    device.printf("Mag Calibration done!\n\r");
    device.printf("x mag bias = %f\n\r", magBias[0]);
    device.printf("y mag bias = %f\n\r", magBias[1]);
    device.printf("z mag bias = %f\n\r", magBias[2]);
    //predicted_pk = A*actual_pk;
    //predicted_pk.print();
    wait(2);
    }
    
   else
   
   {
    device.printf("Could not connect to MPU9250: \n\r");
    device.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=accel_varb;
    }
    if((a3<=0.042)&&a3>=-0.042)
    {
    a3=0;
    accel_var=0;
    }
    else
    {
        accel_var=accel_varb;
    }
    if((a4<=0.035)&&a4>=-0.035)
    {
    a4=0;
    accel_var=0;
    }
    else
    {
     accel_var=accel_varb;
    }
}

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  */
}