Darrien Galid / Mbed 2 deprecated Fused_Boat_Program

Self-navigating boat program with sensors and control system fused

Dependencies:   mbed Servo PID

main.cpp@1:736ae4695570, 2019-05-24 (annotated)

Committer:
Deanatius
Date:
Fri May 24 16:12:40 2019 +0000
Revision:
1:736ae4695570
Parent:
0:cf5854b3296f 
New update to fix motor output

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