bhanu teja / Mbed OS combination

code including all sensors

Dependencies:   LAAP AK8963

main.cpp@2:a79a9c9f83a4, 2019-12-23 (annotated)

Committer:
Glaxys_finest1
Date:
Mon Dec 23 14:45:38 2019 +0000
Revision:
2:a79a9c9f83a4
Parent:
0:66a265dc3146 
workin progress;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Glaxys_finest1 0:66a265dc3146 1 /* mbed Microcontroller Library
Glaxys_finest1 0:66a265dc3146 2 * Copyright (c) 2018 ARM Limited
Glaxys_finest1 0:66a265dc3146 3 * SPDX-License-Identifier: Apache-2.0
Glaxys_finest1 0:66a265dc3146 4 */
Glaxys_finest1 0:66a265dc3146 5
Glaxys_finest1 0:66a265dc3146 6 #include "mbed.h"
Glaxys_finest1 0:66a265dc3146 7 #include "ak8963.h"
Glaxys_finest1 0:66a265dc3146 8 #include <stddef.h>
Glaxys_finest1 0:66a265dc3146 9 #include <stdio.h> // This ert_main.c example uses printf/fflush
Glaxys_finest1 0:66a265dc3146 10 #include "LAAP.h" // Model's header file
Glaxys_finest1 0:66a265dc3146 11 #include "rtwtypes.h"
Glaxys_finest1 0:66a265dc3146 12
Glaxys_finest1 0:66a265dc3146 13 static LAAPModelClass rtObj; // Instance of model class
Glaxys_finest1 0:66a265dc3146 14
Glaxys_finest1 0:66a265dc3146 15 void rt_OneStep(void);
Glaxys_finest1 0:66a265dc3146 16 void rt_OneStep(void)
Glaxys_finest1 0:66a265dc3146 17 {
Glaxys_finest1 0:66a265dc3146 18 static boolean_T OverrunFlag = false;
Glaxys_finest1 0:66a265dc3146 19
Glaxys_finest1 0:66a265dc3146 20 // Disable interrupts here
Glaxys_finest1 0:66a265dc3146 21
Glaxys_finest1 0:66a265dc3146 22 // Check for overrun
Glaxys_finest1 0:66a265dc3146 23 if (OverrunFlag) {
Glaxys_finest1 0:66a265dc3146 24 rtmSetErrorStatus(rtObj.getRTM(), "Overrun");
Glaxys_finest1 0:66a265dc3146 25 return;
Glaxys_finest1 0:66a265dc3146 26 }
Glaxys_finest1 0:66a265dc3146 27
Glaxys_finest1 0:66a265dc3146 28 OverrunFlag = true;
Glaxys_finest1 0:66a265dc3146 29
Glaxys_finest1 0:66a265dc3146 30 // Save FPU context here (if necessary)
Glaxys_finest1 0:66a265dc3146 31 // Re-enable timer or interrupt here
Glaxys_finest1 0:66a265dc3146 32 // Set model inputs here
Glaxys_finest1 0:66a265dc3146 33
Glaxys_finest1 0:66a265dc3146 34 // Step the model
Glaxys_finest1 0:66a265dc3146 35 rtObj.step();
Glaxys_finest1 0:66a265dc3146 36
Glaxys_finest1 0:66a265dc3146 37 // Get model outputs here
Glaxys_finest1 0:66a265dc3146 38
Glaxys_finest1 0:66a265dc3146 39 // Indicate task complete
Glaxys_finest1 0:66a265dc3146 40 OverrunFlag = false;
Glaxys_finest1 0:66a265dc3146 41
Glaxys_finest1 0:66a265dc3146 42 // Disable interrupts here
Glaxys_finest1 0:66a265dc3146 43 // Restore FPU context here (if necessary)
Glaxys_finest1 0:66a265dc3146 44 // Enable interrupts here
Glaxys_finest1 0:66a265dc3146 45 }
Glaxys_finest1 0:66a265dc3146 46 /*
Glaxys_finest1 0:66a265dc3146 47 #define AK8963_ADDRESS 0x0C
Glaxys_finest1 0:66a265dc3146 48 #define WHO_AM_I_AK8963 0x00 // should return 0x48
Glaxys_finest1 0:66a265dc3146 49 #define INFO 0x01
Glaxys_finest1 0:66a265dc3146 50 #define AK8963_ST1 0x02 // data ready status bit 0
Glaxys_finest1 0:66a265dc3146 51 #define AK8963_XOUT_L 0x03 // data
Glaxys_finest1 0:66a265dc3146 52 #define AK8963_XOUT_H 0x04
Glaxys_finest1 0:66a265dc3146 53 #define AK8963_YOUT_L 0x05
Glaxys_finest1 0:66a265dc3146 54 #define AK8963_YOUT_H 0x06
Glaxys_finest1 0:66a265dc3146 55 #define AK8963_ZOUT_L 0x07
Glaxys_finest1 0:66a265dc3146 56 #define AK8963_ZOUT_H 0x08
Glaxys_finest1 0:66a265dc3146 57 #define AK8963_ST2 0x09 // Data overflow bit 3 and data read error status bit 2
Glaxys_finest1 0:66a265dc3146 58 #define AK8963_CNTL 0x0A // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0
Glaxys_finest1 0:66a265dc3146 59 #define AK8963_ASTC 0x0C // Self test control
Glaxys_finest1 0:66a265dc3146 60 #define AK8963_I2CDIS 0x0F // I2C disable
Glaxys_finest1 0:66a265dc3146 61 #define AK8963_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value
Glaxys_finest1 0:66a265dc3146 62 #define AK8963_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value
Glaxys_finest1 0:66a265dc3146 63 #define AK8963_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value
Glaxys_finest1 0:66a265dc3146 64 */
Glaxys_finest1 0:66a265dc3146 65 #define USER_CTRL 0x6A;
Glaxys_finest1 0:66a265dc3146 66 #define I2C_MST_EN 0x20;
Glaxys_finest1 0:66a265dc3146 67
Glaxys_finest1 0:66a265dc3146 68 #define MPU6050_ADDR 0xD0
Glaxys_finest1 0:66a265dc3146 69 #define MPU6050_SMPLRT_DIV 0x19
Glaxys_finest1 0:66a265dc3146 70 #define MPU6050_CONFIG 0x1a
Glaxys_finest1 0:66a265dc3146 71 #define MPU6050_GYRO_CONFIG 0x1b
Glaxys_finest1 0:66a265dc3146 72 #define MPU6050_ACCEL_CONFIG 0x1c
Glaxys_finest1 0:66a265dc3146 73 #define MPU6050_WHO_AM_I 0x75
Glaxys_finest1 0:66a265dc3146 74 #define MPU6050_PWR_MGMT_1 0x6b
Glaxys_finest1 0:66a265dc3146 75 #define ACCEL_XOUT_H_REG 0x3B
Glaxys_finest1 0:66a265dc3146 76 #define Alfa_comp 0.96
Glaxys_finest1 0:66a265dc3146 77
Glaxys_finest1 0:66a265dc3146 78 double Ax, Ay, Az, Gx, Gy, Gz,Mx,My,Mz;
Glaxys_finest1 0:66a265dc3146 79
Glaxys_finest1 0:66a265dc3146 80 double H_senx,H_seny,H_senz;
Glaxys_finest1 0:66a265dc3146 81 char name[1];
Glaxys_finest1 0:66a265dc3146 82
Glaxys_finest1 0:66a265dc3146 83 int16_t Mag_X_RAW = 0;
Glaxys_finest1 0:66a265dc3146 84 int16_t Mag_Y_RAW = 0;
Glaxys_finest1 0:66a265dc3146 85 int16_t Mag_Z_RAW = 0;
Glaxys_finest1 0:66a265dc3146 86
Glaxys_finest1 0:66a265dc3146 87 double avgx,avgy,avgz;
Glaxys_finest1 0:66a265dc3146 88 double rate,bias=0;
Glaxys_finest1 0:66a265dc3146 89 double angle;
Glaxys_finest1 0:66a265dc3146 90
Glaxys_finest1 0:66a265dc3146 91 double pitch_acc=0, roll_acc=0;
Glaxys_finest1 0:66a265dc3146 92 double pitch_gyro=0, roll_gyro=0;
Glaxys_finest1 0:66a265dc3146 93 double pitch=0, roll=0,yaw=0;
Glaxys_finest1 0:66a265dc3146 94 double dtx,dtx1,dtx2,dt=0,dt1,dt2;
Glaxys_finest1 0:66a265dc3146 95 //global variables for meaured pwm values
Glaxys_finest1 0:66a265dc3146 96 uint16_t Rip_chnl_n[6],Rip_chnl_p[6];
Glaxys_finest1 0:66a265dc3146 97
Glaxys_finest1 0:66a265dc3146 98 int distx,disty,distz;
Glaxys_finest1 0:66a265dc3146 99 int distxo,distyo,distzo;
Glaxys_finest1 0:66a265dc3146 100 int velocityx,velocityy,velocityz;
Glaxys_finest1 0:66a265dc3146 101
Glaxys_finest1 0:66a265dc3146 102 //////////////////////To measure pwm using interrupts and timer///////////////////////////////
Glaxys_finest1 0:66a265dc3146 103 InterruptIn ch1(PG_2);
Glaxys_finest1 0:66a265dc3146 104 InterruptIn ch2(PG_3);
Glaxys_finest1 0:66a265dc3146 105 InterruptIn ch3(A4);
Glaxys_finest1 0:66a265dc3146 106 InterruptIn ch4(A5);
Glaxys_finest1 0:66a265dc3146 107 InterruptIn ch5(PG_0);
Glaxys_finest1 0:66a265dc3146 108 InterruptIn ch6(PG_1);
Glaxys_finest1 0:66a265dc3146 109 ///////////////////////////////////////////////////////////////////////////////////////////
Glaxys_finest1 0:66a265dc3146 110 //////////////////// pwm out for ESC inputs////////////////////////////////////////////////
Glaxys_finest1 0:66a265dc3146 111 PwmOut u1(PE_9);
Glaxys_finest1 0:66a265dc3146 112 PwmOut u2(PE_11);
Glaxys_finest1 0:66a265dc3146 113 PwmOut u3(PE_12);
Glaxys_finest1 0:66a265dc3146 114 PwmOut u4(PE_14);
Glaxys_finest1 0:66a265dc3146 115 ///////////////////////////I2C/////////////////////////////////////////////////////////////
Glaxys_finest1 0:66a265dc3146 116 I2C i2c(PF_15,PF_14);
Glaxys_finest1 0:66a265dc3146 117 //////////////////////////////////////////////////////////////////////////////////////////
Glaxys_finest1 0:66a265dc3146 118 ///////////////////////////////LIDAR serial ports/////////////////////////////////////////
Glaxys_finest1 0:66a265dc3146 119 RawSerial rawz(PD_5,PD_6);
Glaxys_finest1 0:66a265dc3146 120 RawSerial rawx(PC_10,PC_11);
Glaxys_finest1 0:66a265dc3146 121 RawSerial rawy(PC_12,PD_2);
Glaxys_finest1 0:66a265dc3146 122 //////////////////////////////////////////////////////////////////////////////////////////
Glaxys_finest1 0:66a265dc3146 123 //////////blue light indicator for error in i2c///////////////////////////////////////////
Glaxys_finest1 0:66a265dc3146 124 DigitalOut myled(LED2);
Glaxys_finest1 0:66a265dc3146 125 //////////////////////////////////////////////////////////////////////////////////////////
Glaxys_finest1 0:66a265dc3146 126 Serial pc(SERIAL_TX, SERIAL_RX);
Glaxys_finest1 0:66a265dc3146 127
Glaxys_finest1 0:66a265dc3146 128 Timeout onestep;
Glaxys_finest1 0:66a265dc3146 129
Glaxys_finest1 0:66a265dc3146 130 Timer timer1,timer2;
Glaxys_finest1 0:66a265dc3146 131 ////////////////////////////////function initializations/////////////////////////////////
Glaxys_finest1 2:a79a9c9f83a4 132
Glaxys_finest1 2:a79a9c9f83a4 133
Glaxys_finest1 2:a79a9c9f83a4 134 int main()
Glaxys_finest1 2:a79a9c9f83a4 135 {
Glaxys_finest1 2:a79a9c9f83a4 136 void stop_counter();
Glaxys_finest1 0:66a265dc3146 137 void start_counter();
Glaxys_finest1 0:66a265dc3146 138 void counters_init();
Glaxys_finest1 0:66a265dc3146 139
Glaxys_finest1 0:66a265dc3146 140 void dist_x();
Glaxys_finest1 0:66a265dc3146 141 void dist_y();
Glaxys_finest1 0:66a265dc3146 142 void dist_z();
Glaxys_finest1 0:66a265dc3146 143
Glaxys_finest1 0:66a265dc3146 144 void mpu_init();
Glaxys_finest1 0:66a265dc3146 145 void mpu_read();
Glaxys_finest1 0:66a265dc3146 146 void get_angle();
Glaxys_finest1 0:66a265dc3146 147 void mpu_calibrate();
Glaxys_finest1 0:66a265dc3146 148 void Mag_init();
Glaxys_finest1 0:66a265dc3146 149 void Mag_read();
Glaxys_finest1 0:66a265dc3146 150
Glaxys_finest1 2:a79a9c9f83a4 151 onestep.attach_us(&rt_OneStep,100);
Glaxys_finest1 0:66a265dc3146 152 counters_init();
Glaxys_finest1 0:66a265dc3146 153
Glaxys_finest1 0:66a265dc3146 154 pc.baud(115200);
Glaxys_finest1 0:66a265dc3146 155
Glaxys_finest1 0:66a265dc3146 156 rawx.baud(115200);
Glaxys_finest1 0:66a265dc3146 157 rawy.baud(115200);
Glaxys_finest1 0:66a265dc3146 158 rawz.baud(115200);
Glaxys_finest1 0:66a265dc3146 159
Glaxys_finest1 0:66a265dc3146 160 rawx.attach(&dist_x);
Glaxys_finest1 0:66a265dc3146 161 rawy.attach(&dist_y);
Glaxys_finest1 0:66a265dc3146 162 rawz.attach(&dist_z);
Glaxys_finest1 0:66a265dc3146 163
Glaxys_finest1 0:66a265dc3146 164 mpu_init();
Glaxys_finest1 0:66a265dc3146 165 //Mag_init();
Glaxys_finest1 0:66a265dc3146 166 /*AK8963 mag(&i2c, AK8963::SLAVE_ADDR_1);
Glaxys_finest1 0:66a265dc3146 167 if(mag.checkConnection() != AK8963::SUCCESS){
Glaxys_finest1 0:66a265dc3146 168 pc.printf("check connection");
Glaxys_finest1 0:66a265dc3146 169
Glaxys_finest1 0:66a265dc3146 170 }
Glaxys_finest1 0:66a265dc3146 171 if(mag.setOperationMode(AK8963::MODE_CONTINUOUS_1) != AK8963::SUCCESS){
Glaxys_finest1 0:66a265dc3146 172 while(1){pc.printf("failed continious mode");}
Glaxys_finest1 0:66a265dc3146 173 }*/
Glaxys_finest1 0:66a265dc3146 174 timer1.start(); //////////////timer for channel
Glaxys_finest1 0:66a265dc3146 175 timer2.start(); /////////////timer for dt to calculate dt
Glaxys_finest1 0:66a265dc3146 176
Glaxys_finest1 0:66a265dc3146 177 //mpu_calibrate();
Glaxys_finest1 0:66a265dc3146 178
Glaxys_finest1 0:66a265dc3146 179
Glaxys_finest1 0:66a265dc3146 180 u1.period_us(20000);
Glaxys_finest1 0:66a265dc3146 181 u2.period_us(20000);
Glaxys_finest1 0:66a265dc3146 182 u3.period_us(20000);
Glaxys_finest1 0:66a265dc3146 183 u4.period_us(20000);
Glaxys_finest1 0:66a265dc3146 184
Glaxys_finest1 0:66a265dc3146 185
Glaxys_finest1 0:66a265dc3146 186 while(timer2.read_ms()<5000)
Glaxys_finest1 0:66a265dc3146 187 { u1.write((float)rtObj.getu1_());
Glaxys_finest1 0:66a265dc3146 188 u2.write((float)rtObj.getu2_());
Glaxys_finest1 0:66a265dc3146 189 u3.write((float)rtObj.getu3 ());
Glaxys_finest1 0:66a265dc3146 190 u4.write((float)rtObj.getu4 ());
Glaxys_finest1 0:66a265dc3146 191
Glaxys_finest1 0:66a265dc3146 192 }
Glaxys_finest1 0:66a265dc3146 193
Glaxys_finest1 0:66a265dc3146 194
Glaxys_finest1 0:66a265dc3146 195
Glaxys_finest1 0:66a265dc3146 196
Glaxys_finest1 0:66a265dc3146 197 while (true)
Glaxys_finest1 0:66a265dc3146 198 { int i;
Glaxys_finest1 0:66a265dc3146 199 //int statusAK8963= AK8963::NOT_DATA_READY;
Glaxys_finest1 0:66a265dc3146 200
Glaxys_finest1 0:66a265dc3146 201
Glaxys_finest1 0:66a265dc3146 202 get_angle();
Glaxys_finest1 0:66a265dc3146 203
Glaxys_finest1 0:66a265dc3146 204 /* if(statusAK8963 == AK8963::DATA_READY){
Glaxys_finest1 0:66a265dc3146 205 AK8963::MagneticVector mag1;
Glaxys_finest1 0:66a265dc3146 206 mag.getMagneticVector(&mag1);
Glaxys_finest1 0:66a265dc3146 207 pc.printf("%5.1f,%5.1f,%5.1f\n",mag1.mx,mag1.my,mag1.mz);
Glaxys_finest1 0:66a265dc3146 208 statusAK8963 = AK8963::NOT_DATA_READY;
Glaxys_finest1 0:66a265dc3146 209 } else if (statusAK8963 == AK8963::NOT_DATA_READY){
Glaxys_finest1 0:66a265dc3146 210 }
Glaxys_finest1 0:66a265dc3146 211
Glaxys_finest1 0:66a265dc3146 212
Glaxys_finest1 0:66a265dc3146 213 for(int i=1000;i<2000;i++){
Glaxys_finest1 0:66a265dc3146 214 u1.write((float)i/20000.0);
Glaxys_finest1 0:66a265dc3146 215 u2.write((float)i/20000.0);
Glaxys_finest1 0:66a265dc3146 216 u3.write((float)i/20000.0);
Glaxys_finest1 0:66a265dc3146 217 u4.write((float)i/20000.0);
Glaxys_finest1 0:66a265dc3146 218 wait(0.0001);}
Glaxys_finest1 0:66a265dc3146 219 for(int i=2000;i>1000;i--){
Glaxys_finest1 0:66a265dc3146 220 u1.write((float)i/20000.0);
Glaxys_finest1 0:66a265dc3146 221 u2.write((float)i/20000.0);
Glaxys_finest1 0:66a265dc3146 222 u3.write((float)i/20000.0);
Glaxys_finest1 0:66a265dc3146 223 u4.write((float)i/20000.0);
Glaxys_finest1 0:66a265dc3146 224 wait(0.0001);}*/
Glaxys_finest1 0:66a265dc3146 225 // Mag_read();
Glaxys_finest1 0:66a265dc3146 226 //for(i=0;i<6;i++)
Glaxys_finest1 0:66a265dc3146 227 //pc.printf("%d\t",Rip_chnl_n[i]);
Glaxys_finest1 0:66a265dc3146 228 // pc.printf("\n");
Glaxys_finest1 0:66a265dc3146 229 /* pc.printf("pitch = %f\t",pitch);
Glaxys_finest1 0:66a265dc3146 230 pc.printf("roll = %f\t",roll);
Glaxys_finest1 0:66a265dc3146 231 pc.printf("yaw = %f\t",yaw);
Glaxys_finest1 0:66a265dc3146 232 */
Glaxys_finest1 0:66a265dc3146 233 pc.printf("x = %d\t",distx);
Glaxys_finest1 0:66a265dc3146 234 pc.printf("y = %d\t",disty);
Glaxys_finest1 0:66a265dc3146 235 pc.printf("z = %d\n",distz);
Glaxys_finest1 0:66a265dc3146 236
Glaxys_finest1 0:66a265dc3146 237
Glaxys_finest1 0:66a265dc3146 238
Glaxys_finest1 0:66a265dc3146 239 //pc.printf("magx = %d\t",name[0]);
Glaxys_finest1 0:66a265dc3146 240 //pc.printf("raw = %d\t",Mag_X_RAW);
Glaxys_finest1 0:66a265dc3146 241 //pc.printf("mx = %f\t",Mx);
Glaxys_finest1 0:66a265dc3146 242 //wait(0.01);
Glaxys_finest1 0:66a265dc3146 243 //
Glaxys_finest1 0:66a265dc3146 244 }
Glaxys_finest1 0:66a265dc3146 245 }
Glaxys_finest1 0:66a265dc3146 246
Glaxys_finest1 0:66a265dc3146 247
Glaxys_finest1 0:66a265dc3146 248 ///////////////////////////////////////////////////////////////functions//////////////////////////////////////////////////////////
Glaxys_finest1 0:66a265dc3146 249
Glaxys_finest1 0:66a265dc3146 250 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Glaxys_finest1 0:66a265dc3146 251
Glaxys_finest1 0:66a265dc3146 252 //////////////////////////////********************************functions for interrupts*****************************////////////////
Glaxys_finest1 0:66a265dc3146 253 void start_counter_1()
Glaxys_finest1 0:66a265dc3146 254 {
Glaxys_finest1 0:66a265dc3146 255 Rip_chnl_p[0]=timer1.read_us();
Glaxys_finest1 0:66a265dc3146 256 }
Glaxys_finest1 0:66a265dc3146 257 void start_counter_2(){
Glaxys_finest1 0:66a265dc3146 258 Rip_chnl_p[1]=timer1.read_us();
Glaxys_finest1 0:66a265dc3146 259 }
Glaxys_finest1 0:66a265dc3146 260 void start_counter_3(){
Glaxys_finest1 0:66a265dc3146 261 Rip_chnl_p[2]=timer1.read_us();
Glaxys_finest1 0:66a265dc3146 262 }
Glaxys_finest1 0:66a265dc3146 263 void start_counter_4(){
Glaxys_finest1 0:66a265dc3146 264 Rip_chnl_p[3]=timer1.read_us();
Glaxys_finest1 0:66a265dc3146 265 }
Glaxys_finest1 0:66a265dc3146 266 void start_counter_5(){
Glaxys_finest1 0:66a265dc3146 267 Rip_chnl_p[4]=timer1.read_us();
Glaxys_finest1 0:66a265dc3146 268 }
Glaxys_finest1 0:66a265dc3146 269 void start_counter_6(){
Glaxys_finest1 0:66a265dc3146 270 Rip_chnl_p[5]=timer1.read_us();
Glaxys_finest1 0:66a265dc3146 271 }
Glaxys_finest1 0:66a265dc3146 272 void stop_counter_1(){
Glaxys_finest1 0:66a265dc3146 273 uint32_t x;
Glaxys_finest1 0:66a265dc3146 274 x=timer1.read_us();
Glaxys_finest1 0:66a265dc3146 275 Rip_chnl_n[0]=x-Rip_chnl_p[0];
Glaxys_finest1 0:66a265dc3146 276 }
Glaxys_finest1 0:66a265dc3146 277 void stop_counter_2(){
Glaxys_finest1 0:66a265dc3146 278 uint32_t x;
Glaxys_finest1 0:66a265dc3146 279 x=timer1.read_us();
Glaxys_finest1 0:66a265dc3146 280 Rip_chnl_n[1]=x-Rip_chnl_p[1];
Glaxys_finest1 0:66a265dc3146 281 }
Glaxys_finest1 0:66a265dc3146 282 void stop_counter_3(){
Glaxys_finest1 0:66a265dc3146 283 uint32_t x;
Glaxys_finest1 0:66a265dc3146 284 x=timer1.read_us();
Glaxys_finest1 0:66a265dc3146 285 Rip_chnl_n[2]=x-Rip_chnl_p[2];
Glaxys_finest1 0:66a265dc3146 286 }
Glaxys_finest1 0:66a265dc3146 287 void stop_counter_4(){
Glaxys_finest1 0:66a265dc3146 288 uint32_t x;
Glaxys_finest1 0:66a265dc3146 289 x=timer1.read_us();
Glaxys_finest1 0:66a265dc3146 290 Rip_chnl_n[3]=x-Rip_chnl_p[3];
Glaxys_finest1 0:66a265dc3146 291 }
Glaxys_finest1 0:66a265dc3146 292 void stop_counter_5(){
Glaxys_finest1 0:66a265dc3146 293 uint32_t x;
Glaxys_finest1 0:66a265dc3146 294 x=timer1.read_us();
Glaxys_finest1 0:66a265dc3146 295 Rip_chnl_n[4]=x-Rip_chnl_p[4];
Glaxys_finest1 0:66a265dc3146 296 }
Glaxys_finest1 0:66a265dc3146 297 void stop_counter_6(){
Glaxys_finest1 0:66a265dc3146 298 uint32_t x;
Glaxys_finest1 0:66a265dc3146 299 x=timer1.read_us();
Glaxys_finest1 0:66a265dc3146 300 Rip_chnl_n[5]=x-Rip_chnl_p[5];
Glaxys_finest1 0:66a265dc3146 301 }
Glaxys_finest1 0:66a265dc3146 302
Glaxys_finest1 0:66a265dc3146 303 void counters_init(){
Glaxys_finest1 0:66a265dc3146 304 ch1.rise(&start_counter_1);
Glaxys_finest1 0:66a265dc3146 305 ch2.rise(&start_counter_2);
Glaxys_finest1 0:66a265dc3146 306 ch3.rise(&start_counter_3);
Glaxys_finest1 0:66a265dc3146 307 ch4.rise(&start_counter_4);
Glaxys_finest1 0:66a265dc3146 308 ch5.rise(&start_counter_5);
Glaxys_finest1 0:66a265dc3146 309 ch6.rise(&start_counter_6);
Glaxys_finest1 0:66a265dc3146 310 ch1.fall(&stop_counter_1);
Glaxys_finest1 0:66a265dc3146 311 ch2.fall(&stop_counter_2);
Glaxys_finest1 0:66a265dc3146 312 ch3.fall(&stop_counter_3);
Glaxys_finest1 0:66a265dc3146 313 ch4.fall(&stop_counter_4);
Glaxys_finest1 0:66a265dc3146 314 ch5.fall(&stop_counter_5);
Glaxys_finest1 0:66a265dc3146 315 ch6.fall(&stop_counter_6);
Glaxys_finest1 0:66a265dc3146 316 }
Glaxys_finest1 0:66a265dc3146 317
Glaxys_finest1 0:66a265dc3146 318
Glaxys_finest1 0:66a265dc3146 319 //////////////////////////////***********************************LIDAR********************************************////////////////
Glaxys_finest1 0:66a265dc3146 320 void dist_x() ///////////*******use this function to get distance it has object of rawserial as argument*******////////////
Glaxys_finest1 0:66a265dc3146 321 { //int distance;//actual distance measurements of LiDAR
Glaxys_finest1 0:66a265dc3146 322 int strength;//signal strength of LiDAR
Glaxys_finest1 0:66a265dc3146 323 int check;//save check value
Glaxys_finest1 0:66a265dc3146 324 int i;
Glaxys_finest1 0:66a265dc3146 325 int uartx[9];//save data measured by LiDAR
Glaxys_finest1 0:66a265dc3146 326
Glaxys_finest1 0:66a265dc3146 327 if (rawx.readable())//check if serial port has data input
Glaxys_finest1 0:66a265dc3146 328 {
Glaxys_finest1 0:66a265dc3146 329 if(rawx.getc()==0x59)//assess data package frame header 0x59
Glaxys_finest1 0:66a265dc3146 330 {
Glaxys_finest1 0:66a265dc3146 331 uartx[0]=0x59;
Glaxys_finest1 0:66a265dc3146 332 if(rawx.getc()==0x59)//assess data package frame header 0x59
Glaxys_finest1 0:66a265dc3146 333 {
Glaxys_finest1 0:66a265dc3146 334 uartx[1]=0x59;
Glaxys_finest1 0:66a265dc3146 335 for(i=2;i<9;i++)//save data in array
Glaxys_finest1 0:66a265dc3146 336 {
Glaxys_finest1 0:66a265dc3146 337 uartx[i]=rawx.getc();
Glaxys_finest1 0:66a265dc3146 338 }
Glaxys_finest1 0:66a265dc3146 339 check=uartx[0]+uartx[1]+uartx[2]+uartx[3]+uartx[4]+uartx[5]+uartx[6]+uartx[7];
Glaxys_finest1 0:66a265dc3146 340 if(uartx[8]==(check&0xff))//verify the received data as per protocol
Glaxys_finest1 0:66a265dc3146 341 {
Glaxys_finest1 0:66a265dc3146 342
Glaxys_finest1 0:66a265dc3146 343
Glaxys_finest1 0:66a265dc3146 344 strength=uartx[4]+uartx[5]*256;//calculate signal strength value
Glaxys_finest1 0:66a265dc3146 345 distx=uartx[2]+uartx[3]*256;//calculate distance value in cm
Glaxys_finest1 0:66a265dc3146 346 //velocityx = distx - distxo; ////velocity in meters
Glaxys_finest1 0:66a265dc3146 347 //distxo = distx;
Glaxys_finest1 0:66a265dc3146 348 }
Glaxys_finest1 0:66a265dc3146 349 }
Glaxys_finest1 0:66a265dc3146 350 }
Glaxys_finest1 0:66a265dc3146 351 }
Glaxys_finest1 0:66a265dc3146 352
Glaxys_finest1 0:66a265dc3146 353 }
Glaxys_finest1 0:66a265dc3146 354 void dist_y() ///////////*******use this function to get distance it has object of rawserial as argument*******////////////
Glaxys_finest1 0:66a265dc3146 355 {
Glaxys_finest1 0:66a265dc3146 356 int strength;//signal strength of LiDAR
Glaxys_finest1 0:66a265dc3146 357 int check;//save check value
Glaxys_finest1 0:66a265dc3146 358 int i;
Glaxys_finest1 0:66a265dc3146 359 int uarty[9];//save data measured by LiDAR
Glaxys_finest1 0:66a265dc3146 360
Glaxys_finest1 0:66a265dc3146 361 if (rawy.readable())//check if serial port has data input
Glaxys_finest1 0:66a265dc3146 362 {
Glaxys_finest1 0:66a265dc3146 363 if(rawy.getc()==0x59)//assess data package frame header 0x59
Glaxys_finest1 0:66a265dc3146 364 {
Glaxys_finest1 0:66a265dc3146 365 uarty[0]=0x59;
Glaxys_finest1 0:66a265dc3146 366 if(rawy.getc()==0x59)//assess data package frame header 0x59
Glaxys_finest1 0:66a265dc3146 367 {
Glaxys_finest1 0:66a265dc3146 368 uarty[1]=0x59;
Glaxys_finest1 0:66a265dc3146 369 for(i=2;i<9;i++)//save data in array
Glaxys_finest1 0:66a265dc3146 370 {
Glaxys_finest1 0:66a265dc3146 371 uarty[i]=rawy.getc();
Glaxys_finest1 0:66a265dc3146 372 }
Glaxys_finest1 0:66a265dc3146 373 check=uarty[0]+uarty[1]+uarty[2]+uarty[3]+uarty[4]+uarty[5]+uarty[6]+uarty[7];
Glaxys_finest1 0:66a265dc3146 374 if(uarty[8]==(check&0xff))//verify the received data as per protocol
Glaxys_finest1 0:66a265dc3146 375 {
Glaxys_finest1 0:66a265dc3146 376
Glaxys_finest1 0:66a265dc3146 377 strength=uarty[4]+uarty[5]*256;//calculate signal strength value
Glaxys_finest1 0:66a265dc3146 378 disty=uarty[2]+uarty[3]*256;//calculate distance value
Glaxys_finest1 0:66a265dc3146 379 //velocityy = disty - distyo; ////velocity in meters
Glaxys_finest1 0:66a265dc3146 380 //distyo = disty;
Glaxys_finest1 0:66a265dc3146 381 }
Glaxys_finest1 0:66a265dc3146 382
Glaxys_finest1 0:66a265dc3146 383 }
Glaxys_finest1 0:66a265dc3146 384 }
Glaxys_finest1 0:66a265dc3146 385 }
Glaxys_finest1 0:66a265dc3146 386
Glaxys_finest1 0:66a265dc3146 387 }
Glaxys_finest1 0:66a265dc3146 388 void dist_z() ///////////*******use this function to get distance it has object of rawserial as argument*******////////////
Glaxys_finest1 0:66a265dc3146 389 {
Glaxys_finest1 0:66a265dc3146 390 int strength;//signal strength of LiDAR
Glaxys_finest1 0:66a265dc3146 391 int check;//save check value
Glaxys_finest1 0:66a265dc3146 392 int i;
Glaxys_finest1 0:66a265dc3146 393 int uartz[9];//save data measured by LiDAR
Glaxys_finest1 0:66a265dc3146 394
Glaxys_finest1 0:66a265dc3146 395 if (rawz.readable())//check if serial port has data input
Glaxys_finest1 0:66a265dc3146 396 {
Glaxys_finest1 0:66a265dc3146 397 if(rawz.getc()==0x59)//assess data package frame header 0x59
Glaxys_finest1 0:66a265dc3146 398 {
Glaxys_finest1 0:66a265dc3146 399 uartz[0]=0x59;
Glaxys_finest1 0:66a265dc3146 400 if(rawz.getc()==0x59)//assess data package frame header 0x59
Glaxys_finest1 0:66a265dc3146 401 {
Glaxys_finest1 0:66a265dc3146 402 uartz[1]=0x59;
Glaxys_finest1 0:66a265dc3146 403 for(i=2;i<9;i++)//save data in array
Glaxys_finest1 0:66a265dc3146 404 {
Glaxys_finest1 0:66a265dc3146 405 uartz[i]=rawz.getc();
Glaxys_finest1 0:66a265dc3146 406 }
Glaxys_finest1 0:66a265dc3146 407 check=uartz[0]+uartz[1]+uartz[2]+uartz[3]+uartz[4]+uartz[5]+uartz[6]+uartz[7];
Glaxys_finest1 0:66a265dc3146 408 if(uartz[8]==(check&0xff))//verify the received data as per protocol
Glaxys_finest1 0:66a265dc3146 409 {
Glaxys_finest1 0:66a265dc3146 410 strength=uartz[4]+uartz[5]*256;//calculate signal strength value
Glaxys_finest1 0:66a265dc3146 411 distz=uartz[2]+uartz[3]*256;//calculate distance value
Glaxys_finest1 0:66a265dc3146 412 //velocityz = distz - distzo; ////velocity in meters
Glaxys_finest1 0:66a265dc3146 413 //distzo = distz;
Glaxys_finest1 0:66a265dc3146 414 }
Glaxys_finest1 0:66a265dc3146 415 }
Glaxys_finest1 0:66a265dc3146 416 }
Glaxys_finest1 0:66a265dc3146 417 }
Glaxys_finest1 0:66a265dc3146 418
Glaxys_finest1 0:66a265dc3146 419 }
Glaxys_finest1 0:66a265dc3146 420 //////////////////////////////////**********velocity**************//////////////////////////////////////
Glaxys_finest1 0:66a265dc3146 421
Glaxys_finest1 0:66a265dc3146 422
Glaxys_finest1 0:66a265dc3146 423 /////////////////////////////////***********************mpu****************************///////////////////////////////
Glaxys_finest1 0:66a265dc3146 424
Glaxys_finest1 0:66a265dc3146 425 void mpu_init()
Glaxys_finest1 0:66a265dc3146 426 {
Glaxys_finest1 0:66a265dc3146 427 i2c.frequency(100000);
Glaxys_finest1 0:66a265dc3146 428 char data_write[2];
Glaxys_finest1 0:66a265dc3146 429
Glaxys_finest1 0:66a265dc3146 430 data_write[0] = MPU6050_PWR_MGMT_1;
Glaxys_finest1 0:66a265dc3146 431 data_write[1] = 0x00;
Glaxys_finest1 0:66a265dc3146 432
Glaxys_finest1 0:66a265dc3146 433 int status = i2c.write(MPU6050_ADDR, data_write, 2, 0);
Glaxys_finest1 0:66a265dc3146 434
Glaxys_finest1 0:66a265dc3146 435 if (status != 0)
Glaxys_finest1 0:66a265dc3146 436 { // Error
Glaxys_finest1 0:66a265dc3146 437 while (1)
Glaxys_finest1 0:66a265dc3146 438 {
Glaxys_finest1 0:66a265dc3146 439 myled = !myled;
Glaxys_finest1 0:66a265dc3146 440 wait(0.2);
Glaxys_finest1 0:66a265dc3146 441 }
Glaxys_finest1 0:66a265dc3146 442 }
Glaxys_finest1 0:66a265dc3146 443 data_write[0] = USER_CTRL;
Glaxys_finest1 0:66a265dc3146 444 data_write[1] = I2C_MST_EN;
Glaxys_finest1 0:66a265dc3146 445 i2c.write(MPU6050_ADDR, data_write, 2, 0);
Glaxys_finest1 0:66a265dc3146 446
Glaxys_finest1 0:66a265dc3146 447 data_write[0] = 0x37;//INT_PIN_CFG;
Glaxys_finest1 0:66a265dc3146 448 data_write[1] = 0x02;
Glaxys_finest1 0:66a265dc3146 449 i2c.write(MPU6050_ADDR, data_write, 2, 0);
Glaxys_finest1 0:66a265dc3146 450
Glaxys_finest1 0:66a265dc3146 451 data_write[0] = MPU6050_SMPLRT_DIV;
Glaxys_finest1 0:66a265dc3146 452 data_write[1] = 0x07;
Glaxys_finest1 0:66a265dc3146 453 i2c.write(MPU6050_ADDR, data_write, 2, 0);
Glaxys_finest1 0:66a265dc3146 454
Glaxys_finest1 0:66a265dc3146 455 data_write[0] = MPU6050_ACCEL_CONFIG;
Glaxys_finest1 0:66a265dc3146 456 data_write[1] = 0x00;
Glaxys_finest1 0:66a265dc3146 457 i2c.write(MPU6050_ADDR, data_write, 2, 0);
Glaxys_finest1 0:66a265dc3146 458 data_write[0] = 0x1d;
Glaxys_finest1 0:66a265dc3146 459 data_write[1] = 0x06;
Glaxys_finest1 0:66a265dc3146 460 i2c.write(MPU6050_ADDR, data_write, 2, 0);
Glaxys_finest1 0:66a265dc3146 461 data_write[0] = MPU6050_CONFIG;
Glaxys_finest1 0:66a265dc3146 462 data_write[1] = 0x00;
Glaxys_finest1 0:66a265dc3146 463 i2c.write(MPU6050_ADDR, data_write, 2, 0);
Glaxys_finest1 0:66a265dc3146 464
Glaxys_finest1 0:66a265dc3146 465 data_write[0] = MPU6050_GYRO_CONFIG;
Glaxys_finest1 0:66a265dc3146 466 data_write[1] = 0x00;
Glaxys_finest1 0:66a265dc3146 467 i2c.write(MPU6050_ADDR, data_write, 2, 0);
Glaxys_finest1 0:66a265dc3146 468 wait(0.2);
Glaxys_finest1 0:66a265dc3146 469 }
Glaxys_finest1 0:66a265dc3146 470
Glaxys_finest1 0:66a265dc3146 471 void mpu_read(){
Glaxys_finest1 0:66a265dc3146 472 char Rec_Data[14];
Glaxys_finest1 0:66a265dc3146 473 int16_t Accel_X_RAW = 0;
Glaxys_finest1 0:66a265dc3146 474 int16_t Accel_Y_RAW = 0;
Glaxys_finest1 0:66a265dc3146 475 int16_t Accel_Z_RAW = 0;
Glaxys_finest1 0:66a265dc3146 476 int16_t Temp_raw = 0;
Glaxys_finest1 0:66a265dc3146 477 int16_t Gyro_X_RAW = 0;
Glaxys_finest1 0:66a265dc3146 478 int16_t Gyro_Y_RAW = 0;
Glaxys_finest1 0:66a265dc3146 479 int16_t Gyro_Z_RAW = 0;
Glaxys_finest1 0:66a265dc3146 480 char data_write[1];
Glaxys_finest1 0:66a265dc3146 481
Glaxys_finest1 0:66a265dc3146 482 // Read 1 BYTES of data starting from ACCEL_XOUT_H register
Glaxys_finest1 0:66a265dc3146 483 data_write[0] = ACCEL_XOUT_H_REG;
Glaxys_finest1 0:66a265dc3146 484 i2c.write(MPU6050_ADDR, data_write, 1, 1); // no stop
Glaxys_finest1 0:66a265dc3146 485 i2c.read(MPU6050_ADDR, Rec_Data, 14);
Glaxys_finest1 0:66a265dc3146 486 Accel_X_RAW = (int16_t)(Rec_Data[0] << 8 | Rec_Data [1]);
Glaxys_finest1 0:66a265dc3146 487 Accel_Y_RAW = (int16_t)(Rec_Data[2] << 8 | Rec_Data [3]);
Glaxys_finest1 0:66a265dc3146 488 Accel_Z_RAW = (int16_t)(Rec_Data[4] << 8 | Rec_Data [5]);
Glaxys_finest1 0:66a265dc3146 489
Glaxys_finest1 0:66a265dc3146 490 Temp_raw = (int16_t)(Rec_Data[6] << 8 | Rec_Data [7]);
Glaxys_finest1 0:66a265dc3146 491
Glaxys_finest1 0:66a265dc3146 492 Gyro_X_RAW = (int16_t)(Rec_Data[8] << 8 | Rec_Data [9]);
Glaxys_finest1 0:66a265dc3146 493 Gyro_Y_RAW = (int16_t)(Rec_Data[10] << 8 | Rec_Data [11]);
Glaxys_finest1 0:66a265dc3146 494 Gyro_Z_RAW = (int16_t)(Rec_Data[12] << 8 | Rec_Data [13]);
Glaxys_finest1 0:66a265dc3146 495 /*** convert the RAW values into acceleration in 'g'
Glaxys_finest1 0:66a265dc3146 496 we have to divide according to the Full scale value set in FS_SEL
Glaxys_finest1 0:66a265dc3146 497 I have configured FS_SEL = 0. So I am dividing by 16384.0
Glaxys_finest1 0:66a265dc3146 498 for more details check ACCEL_CONFIG Register ****/
Glaxys_finest1 0:66a265dc3146 499
Glaxys_finest1 0:66a265dc3146 500 Ax = Accel_X_RAW/16384.0;
Glaxys_finest1 0:66a265dc3146 501 Ay = Accel_Y_RAW/16384.0;
Glaxys_finest1 0:66a265dc3146 502 Az = Accel_Z_RAW/16384.0;
Glaxys_finest1 0:66a265dc3146 503
Glaxys_finest1 0:66a265dc3146 504 Gx = Gyro_X_RAW/131.0;
Glaxys_finest1 0:66a265dc3146 505 Gy = Gyro_Y_RAW/131.0;
Glaxys_finest1 0:66a265dc3146 506 Gz = Gyro_Z_RAW/131.0;
Glaxys_finest1 0:66a265dc3146 507
Glaxys_finest1 0:66a265dc3146 508 }
Glaxys_finest1 0:66a265dc3146 509
Glaxys_finest1 0:66a265dc3146 510
Glaxys_finest1 0:66a265dc3146 511 void get_angle(){
Glaxys_finest1 0:66a265dc3146 512
Glaxys_finest1 0:66a265dc3146 513 mpu_read();
Glaxys_finest1 0:66a265dc3146 514 dt2=timer2.read_ms(); ////////****very imp self note check the order of dt1 and dt2*********///////////
Glaxys_finest1 0:66a265dc3146 515 dt=dt2-dt1;
Glaxys_finest1 0:66a265dc3146 516 dt=dt/1000;
Glaxys_finest1 0:66a265dc3146 517
Glaxys_finest1 0:66a265dc3146 518 roll_acc = (atan2((Ay),sqrt(pow((Ax),2) + pow((Az),2))))*(90*7/11);
Glaxys_finest1 0:66a265dc3146 519 pitch_acc = ((atan2((-Ax),sqrt(pow((Ay),2) + pow((Az),2))))*(90*7/11));
Glaxys_finest1 0:66a265dc3146 520
Glaxys_finest1 0:66a265dc3146 521 /* COMPLIMENTARY FILTER*/
Glaxys_finest1 0:66a265dc3146 522 pitch = Alfa_comp*(pitch + Gy*dt) + (1-Alfa_comp)*pitch_acc ;
Glaxys_finest1 0:66a265dc3146 523
Glaxys_finest1 0:66a265dc3146 524 roll = Alfa_comp*(roll + Gx*dt) + (1-Alfa_comp)*roll_acc ;
Glaxys_finest1 0:66a265dc3146 525 if(abs(Gz)>1)
Glaxys_finest1 0:66a265dc3146 526 yaw += Gz*dt;
Glaxys_finest1 0:66a265dc3146 527 dt1=timer2.read_ms();
Glaxys_finest1 0:66a265dc3146 528
Glaxys_finest1 0:66a265dc3146 529 }
Glaxys_finest1 0:66a265dc3146 530 void mpu_calibrate(void){
Glaxys_finest1 0:66a265dc3146 531 int i;
Glaxys_finest1 0:66a265dc3146 532 for(i=0;i<=3000;i++)
Glaxys_finest1 0:66a265dc3146 533 {
Glaxys_finest1 0:66a265dc3146 534 //dt2=timer2.read_ms();
Glaxys_finest1 0:66a265dc3146 535 if(i>0){
Glaxys_finest1 0:66a265dc3146 536 get_angle();}
Glaxys_finest1 0:66a265dc3146 537
Glaxys_finest1 0:66a265dc3146 538 // dt1=timer2.read_ms();
Glaxys_finest1 0:66a265dc3146 539 //dt=dt1-dt2;
Glaxys_finest1 0:66a265dc3146 540
Glaxys_finest1 0:66a265dc3146 541 avgx+=roll;
Glaxys_finest1 0:66a265dc3146 542 avgy+=pitch;
Glaxys_finest1 0:66a265dc3146 543 //avgz+=yaw;
Glaxys_finest1 0:66a265dc3146 544 }
Glaxys_finest1 0:66a265dc3146 545 avgx=avgx/3000;
Glaxys_finest1 0:66a265dc3146 546 avgy=avgy/3000;
Glaxys_finest1 0:66a265dc3146 547 // avgz=avgz/2000;
Glaxys_finest1 0:66a265dc3146 548 }
Glaxys_finest1 0:66a265dc3146 549 /*void Mag_init(){
Glaxys_finest1 0:66a265dc3146 550 char raw[3];
Glaxys_finest1 0:66a265dc3146 551 char data_write[2];
Glaxys_finest1 0:66a265dc3146 552
Glaxys_finest1 0:66a265dc3146 553 ///////////////////////////////////////// Power down magnetometer///////////////////////////////////////////////////////
Glaxys_finest1 0:66a265dc3146 554 data_write[0] = AK8963_CNTL;
Glaxys_finest1 0:66a265dc3146 555 data_write[1] = 0x00;
Glaxys_finest1 0:66a265dc3146 556
Glaxys_finest1 0:66a265dc3146 557 int status = i2c.write(AK8963_ADDRESS, data_write, 2, 0);
Glaxys_finest1 0:66a265dc3146 558 if (status != 0)
Glaxys_finest1 0:66a265dc3146 559 { // Error
Glaxys_finest1 0:66a265dc3146 560 while (1)
Glaxys_finest1 0:66a265dc3146 561 {
Glaxys_finest1 0:66a265dc3146 562 myled = !myled;
Glaxys_finest1 0:66a265dc3146 563 wait(0.2);
Glaxys_finest1 0:66a265dc3146 564 }
Glaxys_finest1 0:66a265dc3146 565 }
Glaxys_finest1 0:66a265dc3146 566
Glaxys_finest1 0:66a265dc3146 567 wait(0.01);
Glaxys_finest1 0:66a265dc3146 568 data_write[0] = 0x6B; //////////////////rest magnetometer
Glaxys_finest1 0:66a265dc3146 569 data_write[1] = 0x80;
Glaxys_finest1 0:66a265dc3146 570
Glaxys_finest1 0:66a265dc3146 571 i2c.write(MPU6050_ADDR, data_write, 2, 0);
Glaxys_finest1 0:66a265dc3146 572
Glaxys_finest1 0:66a265dc3146 573
Glaxys_finest1 0:66a265dc3146 574 wait(0.01);
Glaxys_finest1 0:66a265dc3146 575 ///////////////////////////////////////// Enter Fuse ROM access mode///////////////////////////////////////////////////
Glaxys_finest1 0:66a265dc3146 576 data_write[0] = AK8963_CNTL;
Glaxys_finest1 0:66a265dc3146 577 data_write[1] = 0x0F;
Glaxys_finest1 0:66a265dc3146 578 i2c.write(AK8963_ADDRESS, data_write, 2, 0);
Glaxys_finest1 0:66a265dc3146 579
Glaxys_finest1 0:66a265dc3146 580 wait(0.01);
Glaxys_finest1 0:66a265dc3146 581 ///////////////////////////////////////// Read the x-, y-, and z-axis calibration values//////////////////////////////
Glaxys_finest1 0:66a265dc3146 582 data_write[0] = AK8963_ASAX;
Glaxys_finest1 0:66a265dc3146 583 i2c.write(AK8963_ADDRESS, data_write, 1, 0);
Glaxys_finest1 0:66a265dc3146 584 i2c.read(AK8963_ADDRESS, raw, 3);
Glaxys_finest1 0:66a265dc3146 585 // Return x-axis sensitivity adjustment values, etc.
Glaxys_finest1 0:66a265dc3146 586 H_senx = (float)(raw[0] - 128)/256.0f + 1.0f;
Glaxys_finest1 0:66a265dc3146 587 H_seny = (float)(raw[1] - 128)/256.0f + 1.0f;
Glaxys_finest1 0:66a265dc3146 588 H_senz = (float)(raw[2] - 128)/256.0f + 1.0f;
Glaxys_finest1 0:66a265dc3146 589
Glaxys_finest1 0:66a265dc3146 590 data_write[0] = AK8963_CNTL;
Glaxys_finest1 0:66a265dc3146 591 data_write[1] = 0x16;
Glaxys_finest1 0:66a265dc3146 592 i2c.write(AK8963_ADDRESS, data_write, 2, 0);
Glaxys_finest1 0:66a265dc3146 593 wait(0.01);
Glaxys_finest1 0:66a265dc3146 594 data_write[0] = 0x0B;
Glaxys_finest1 0:66a265dc3146 595 data_write[1] = 0x01;
Glaxys_finest1 0:66a265dc3146 596 i2c.write(AK8963_ADDRESS, data_write, 2, 0);
Glaxys_finest1 0:66a265dc3146 597 wait(0.01);
Glaxys_finest1 0:66a265dc3146 598 // Configure the magnetometer for continuous read and highest resolution
Glaxys_finest1 0:66a265dc3146 599 // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register,
Glaxys_finest1 0:66a265dc3146 600 // and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates
Glaxys_finest1 0:66a265dc3146 601
Glaxys_finest1 0:66a265dc3146 602 }
Glaxys_finest1 0:66a265dc3146 603 void Mag_read(){
Glaxys_finest1 0:66a265dc3146 604
Glaxys_finest1 0:66a265dc3146 605 char data_write[1];
Glaxys_finest1 0:66a265dc3146 606 char x[1];
Glaxys_finest1 0:66a265dc3146 607 char rawData[7];
Glaxys_finest1 0:66a265dc3146 608 /////////////read name///////////////////////////////////
Glaxys_finest1 0:66a265dc3146 609 data_write[0] = WHO_AM_I_AK8963;
Glaxys_finest1 0:66a265dc3146 610 i2c.write(AK8963_ADDRESS, data_write, 1, 0);
Glaxys_finest1 0:66a265dc3146 611 i2c.read(AK8963_ADDRESS, name, 1);
Glaxys_finest1 0:66a265dc3146 612
Glaxys_finest1 0:66a265dc3146 613 //////////////////////////////////////
Glaxys_finest1 0:66a265dc3146 614 data_write[0] = AK8963_ST1;
Glaxys_finest1 0:66a265dc3146 615 i2c.write(AK8963_ADDRESS, data_write, 1, 0);
Glaxys_finest1 0:66a265dc3146 616 i2c.read(AK8963_ADDRESS, x, 1);
Glaxys_finest1 0:66a265dc3146 617
Glaxys_finest1 0:66a265dc3146 618 if(x[0] & 0x01) { // wait for magnetometer data ready bit to be set
Glaxys_finest1 0:66a265dc3146 619 data_write[0] = AK8963_XOUT_L;// Read the six raw data and ST2 registers sequentially into data array
Glaxys_finest1 0:66a265dc3146 620 i2c.write(AK8963_ADDRESS, data_write, 1, 1);
Glaxys_finest1 0:66a265dc3146 621 i2c.read(AK8963_ADDRESS, rawData, 7);
Glaxys_finest1 0:66a265dc3146 622
Glaxys_finest1 0:66a265dc3146 623 uint8_t c = rawData[6]; // End data read by reading ST2 register
Glaxys_finest1 0:66a265dc3146 624 if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data
Glaxys_finest1 0:66a265dc3146 625 Mag_X_RAW = (int16_t)((rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value
Glaxys_finest1 0:66a265dc3146 626 Mag_Y_RAW = (int16_t)((rawData[3] << 8) | rawData[2]) ; // Data stored as little Endian
Glaxys_finest1 0:66a265dc3146 627 Mag_Z_RAW = (int16_t)((rawData[5] << 8) | rawData[4]) ;
Glaxys_finest1 0:66a265dc3146 628 }
Glaxys_finest1 0:66a265dc3146 629 }
Glaxys_finest1 0:66a265dc3146 630 Mx = Mag_X_RAW*H_senx;
Glaxys_finest1 0:66a265dc3146 631 My = Mag_Y_RAW*H_seny;
Glaxys_finest1 0:66a265dc3146 632 Mz = Mag_Z_RAW*H_senz;
Glaxys_finest1 0:66a265dc3146 633 }*/