Dragon / Mbed OS combination

code including all sensors

Dependencies:   LAAP AK8963

main.cpp@1:3463231e9c41, 2019-11-25 (annotated)

Committer:
Glaxys_finest1
Date:
Mon Nov 25 10:53:05 2019 +0000
Revision:
1:3463231e9c41
Parent:
0:66a265dc3146 
hello1

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 0:66a265dc3146 132 void stop_counter();
Glaxys_finest1 0:66a265dc3146 133 void start_counter();
Glaxys_finest1 0:66a265dc3146 134 void counters_init();
Glaxys_finest1 0:66a265dc3146 135
Glaxys_finest1 0:66a265dc3146 136 void dist_x();
Glaxys_finest1 0:66a265dc3146 137 void dist_y();
Glaxys_finest1 0:66a265dc3146 138 void dist_z();
Glaxys_finest1 0:66a265dc3146 139
Glaxys_finest1 0:66a265dc3146 140 void mpu_init();
Glaxys_finest1 0:66a265dc3146 141 void mpu_read();
Glaxys_finest1 0:66a265dc3146 142 void get_angle();
Glaxys_finest1 0:66a265dc3146 143 void mpu_calibrate();
Glaxys_finest1 0:66a265dc3146 144 void Mag_init();
Glaxys_finest1 0:66a265dc3146 145 void Mag_read();
Glaxys_finest1 0:66a265dc3146 146
Glaxys_finest1 0:66a265dc3146 147 onestep.attach_us(&rt_OneStep(),100);
Glaxys_finest1 0:66a265dc3146 148
Glaxys_finest1 0:66a265dc3146 149 int main()
Glaxys_finest1 0:66a265dc3146 150 {
Glaxys_finest1 0:66a265dc3146 151 counters_init();
Glaxys_finest1 0:66a265dc3146 152
Glaxys_finest1 0:66a265dc3146 153 pc.baud(115200);
Glaxys_finest1 0:66a265dc3146 154
Glaxys_finest1 0:66a265dc3146 155 rawx.baud(115200);
Glaxys_finest1 0:66a265dc3146 156 rawy.baud(115200);
Glaxys_finest1 0:66a265dc3146 157 rawz.baud(115200);
Glaxys_finest1 0:66a265dc3146 158
Glaxys_finest1 0:66a265dc3146 159 rawx.attach(&dist_x);
Glaxys_finest1 0:66a265dc3146 160 rawy.attach(&dist_y);
Glaxys_finest1 0:66a265dc3146 161 rawz.attach(&dist_z);
Glaxys_finest1 0:66a265dc3146 162
Glaxys_finest1 0:66a265dc3146 163 mpu_init();
Glaxys_finest1 0:66a265dc3146 164 //Mag_init();
Glaxys_finest1 0:66a265dc3146 165 /*AK8963 mag(&i2c, AK8963::SLAVE_ADDR_1);
Glaxys_finest1 0:66a265dc3146 166 if(mag.checkConnection() != AK8963::SUCCESS){
Glaxys_finest1 0:66a265dc3146 167 pc.printf("check connection");
Glaxys_finest1 0:66a265dc3146 168
Glaxys_finest1 0:66a265dc3146 169 }
Glaxys_finest1 0:66a265dc3146 170 if(mag.setOperationMode(AK8963::MODE_CONTINUOUS_1) != AK8963::SUCCESS){
Glaxys_finest1 0:66a265dc3146 171 while(1){pc.printf("failed continious mode");}
Glaxys_finest1 0:66a265dc3146 172 }*/
Glaxys_finest1 0:66a265dc3146 173 timer1.start(); //////////////timer for channel
Glaxys_finest1 0:66a265dc3146 174 timer2.start(); /////////////timer for dt to calculate dt
Glaxys_finest1 0:66a265dc3146 175
Glaxys_finest1 0:66a265dc3146 176 //mpu_calibrate();
Glaxys_finest1 0:66a265dc3146 177
Glaxys_finest1 0:66a265dc3146 178
Glaxys_finest1 0:66a265dc3146 179 u1.period_us(20000);
Glaxys_finest1 0:66a265dc3146 180 u2.period_us(20000);
Glaxys_finest1 0:66a265dc3146 181 u3.period_us(20000);
Glaxys_finest1 0:66a265dc3146 182 u4.period_us(20000);
Glaxys_finest1 0:66a265dc3146 183
Glaxys_finest1 0:66a265dc3146 184
Glaxys_finest1 0:66a265dc3146 185 while(timer2.read_ms()<5000)
Glaxys_finest1 0:66a265dc3146 186 { u1.write((float)rtObj.getu1_());
Glaxys_finest1 0:66a265dc3146 187 u2.write((float)rtObj.getu2_());
Glaxys_finest1 0:66a265dc3146 188 u3.write((float)rtObj.getu3 ());
Glaxys_finest1 0:66a265dc3146 189 u4.write((float)rtObj.getu4 ());
Glaxys_finest1 0:66a265dc3146 190
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 while (true)
Glaxys_finest1 0:66a265dc3146 197 { int i;
Glaxys_finest1 0:66a265dc3146 198 //int statusAK8963= AK8963::NOT_DATA_READY;
Glaxys_finest1 0:66a265dc3146 199
Glaxys_finest1 0:66a265dc3146 200
Glaxys_finest1 0:66a265dc3146 201 get_angle();
Glaxys_finest1 0:66a265dc3146 202
Glaxys_finest1 0:66a265dc3146 203 /* if(statusAK8963 == AK8963::DATA_READY){
Glaxys_finest1 0:66a265dc3146 204 AK8963::MagneticVector mag1;
Glaxys_finest1 0:66a265dc3146 205 mag.getMagneticVector(&mag1);
Glaxys_finest1 0:66a265dc3146 206 pc.printf("%5.1f,%5.1f,%5.1f\n",mag1.mx,mag1.my,mag1.mz);
Glaxys_finest1 0:66a265dc3146 207 statusAK8963 = AK8963::NOT_DATA_READY;
Glaxys_finest1 0:66a265dc3146 208 } else if (statusAK8963 == AK8963::NOT_DATA_READY){
Glaxys_finest1 0:66a265dc3146 209 }
Glaxys_finest1 0:66a265dc3146 210
Glaxys_finest1 0:66a265dc3146 211
Glaxys_finest1 0:66a265dc3146 212 for(int i=1000;i<2000;i++){
Glaxys_finest1 0:66a265dc3146 213 u1.write((float)i/20000.0);
Glaxys_finest1 0:66a265dc3146 214 u2.write((float)i/20000.0);
Glaxys_finest1 0:66a265dc3146 215 u3.write((float)i/20000.0);
Glaxys_finest1 0:66a265dc3146 216 u4.write((float)i/20000.0);
Glaxys_finest1 0:66a265dc3146 217 wait(0.0001);}
Glaxys_finest1 0:66a265dc3146 218 for(int i=2000;i>1000;i--){
Glaxys_finest1 0:66a265dc3146 219 u1.write((float)i/20000.0);
Glaxys_finest1 0:66a265dc3146 220 u2.write((float)i/20000.0);
Glaxys_finest1 0:66a265dc3146 221 u3.write((float)i/20000.0);
Glaxys_finest1 0:66a265dc3146 222 u4.write((float)i/20000.0);
Glaxys_finest1 0:66a265dc3146 223 wait(0.0001);}*/
Glaxys_finest1 0:66a265dc3146 224 // Mag_read();
Glaxys_finest1 0:66a265dc3146 225 //for(i=0;i<6;i++)
Glaxys_finest1 0:66a265dc3146 226 //pc.printf("%d\t",Rip_chnl_n[i]);
Glaxys_finest1 0:66a265dc3146 227 // pc.printf("\n");
Glaxys_finest1 0:66a265dc3146 228 /* pc.printf("pitch = %f\t",pitch);
Glaxys_finest1 0:66a265dc3146 229 pc.printf("roll = %f\t",roll);
Glaxys_finest1 0:66a265dc3146 230 pc.printf("yaw = %f\t",yaw);
Glaxys_finest1 0:66a265dc3146 231 */
Glaxys_finest1 0:66a265dc3146 232 pc.printf("x = %d\t",distx);
Glaxys_finest1 0:66a265dc3146 233 pc.printf("y = %d\t",disty);
Glaxys_finest1 0:66a265dc3146 234 pc.printf("z = %d\n",distz);
Glaxys_finest1 0:66a265dc3146 235
Glaxys_finest1 0:66a265dc3146 236
Glaxys_finest1 0:66a265dc3146 237
Glaxys_finest1 0:66a265dc3146 238 //pc.printf("magx = %d\t",name[0]);
Glaxys_finest1 0:66a265dc3146 239 //pc.printf("raw = %d\t",Mag_X_RAW);
Glaxys_finest1 0:66a265dc3146 240 //pc.printf("mx = %f\t",Mx);
Glaxys_finest1 0:66a265dc3146 241 //wait(0.01);
Glaxys_finest1 0:66a265dc3146 242 //
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 ///////////////////////////////////////////////////////////////functions//////////////////////////////////////////////////////////
Glaxys_finest1 0:66a265dc3146 248
Glaxys_finest1 0:66a265dc3146 249 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Glaxys_finest1 0:66a265dc3146 250
Glaxys_finest1 0:66a265dc3146 251 //////////////////////////////********************************functions for interrupts*****************************////////////////
Glaxys_finest1 0:66a265dc3146 252 void start_counter_1()
Glaxys_finest1 0:66a265dc3146 253 {
Glaxys_finest1 0:66a265dc3146 254 Rip_chnl_p[0]=timer1.read_us();
Glaxys_finest1 0:66a265dc3146 255 }
Glaxys_finest1 0:66a265dc3146 256 void start_counter_2(){
Glaxys_finest1 0:66a265dc3146 257 Rip_chnl_p[1]=timer1.read_us();
Glaxys_finest1 0:66a265dc3146 258 }
Glaxys_finest1 0:66a265dc3146 259 void start_counter_3(){
Glaxys_finest1 0:66a265dc3146 260 Rip_chnl_p[2]=timer1.read_us();
Glaxys_finest1 0:66a265dc3146 261 }
Glaxys_finest1 0:66a265dc3146 262 void start_counter_4(){
Glaxys_finest1 0:66a265dc3146 263 Rip_chnl_p[3]=timer1.read_us();
Glaxys_finest1 0:66a265dc3146 264 }
Glaxys_finest1 0:66a265dc3146 265 void start_counter_5(){
Glaxys_finest1 0:66a265dc3146 266 Rip_chnl_p[4]=timer1.read_us();
Glaxys_finest1 0:66a265dc3146 267 }
Glaxys_finest1 0:66a265dc3146 268 void start_counter_6(){
Glaxys_finest1 0:66a265dc3146 269 Rip_chnl_p[5]=timer1.read_us();
Glaxys_finest1 0:66a265dc3146 270 }
Glaxys_finest1 0:66a265dc3146 271 void stop_counter_1(){
Glaxys_finest1 0:66a265dc3146 272 uint32_t x;
Glaxys_finest1 0:66a265dc3146 273 x=timer1.read_us();
Glaxys_finest1 0:66a265dc3146 274 Rip_chnl_n[0]=x-Rip_chnl_p[0];
Glaxys_finest1 0:66a265dc3146 275 }
Glaxys_finest1 0:66a265dc3146 276 void stop_counter_2(){
Glaxys_finest1 0:66a265dc3146 277 uint32_t x;
Glaxys_finest1 0:66a265dc3146 278 x=timer1.read_us();
Glaxys_finest1 0:66a265dc3146 279 Rip_chnl_n[1]=x-Rip_chnl_p[1];
Glaxys_finest1 0:66a265dc3146 280 }
Glaxys_finest1 0:66a265dc3146 281 void stop_counter_3(){
Glaxys_finest1 0:66a265dc3146 282 uint32_t x;
Glaxys_finest1 0:66a265dc3146 283 x=timer1.read_us();
Glaxys_finest1 0:66a265dc3146 284 Rip_chnl_n[2]=x-Rip_chnl_p[2];
Glaxys_finest1 0:66a265dc3146 285 }
Glaxys_finest1 0:66a265dc3146 286 void stop_counter_4(){
Glaxys_finest1 0:66a265dc3146 287 uint32_t x;
Glaxys_finest1 0:66a265dc3146 288 x=timer1.read_us();
Glaxys_finest1 0:66a265dc3146 289 Rip_chnl_n[3]=x-Rip_chnl_p[3];
Glaxys_finest1 0:66a265dc3146 290 }
Glaxys_finest1 0:66a265dc3146 291 void stop_counter_5(){
Glaxys_finest1 0:66a265dc3146 292 uint32_t x;
Glaxys_finest1 0:66a265dc3146 293 x=timer1.read_us();
Glaxys_finest1 0:66a265dc3146 294 Rip_chnl_n[4]=x-Rip_chnl_p[4];
Glaxys_finest1 0:66a265dc3146 295 }
Glaxys_finest1 0:66a265dc3146 296 void stop_counter_6(){
Glaxys_finest1 0:66a265dc3146 297 uint32_t x;
Glaxys_finest1 0:66a265dc3146 298 x=timer1.read_us();
Glaxys_finest1 0:66a265dc3146 299 Rip_chnl_n[5]=x-Rip_chnl_p[5];
Glaxys_finest1 0:66a265dc3146 300 }
Glaxys_finest1 0:66a265dc3146 301
Glaxys_finest1 0:66a265dc3146 302 void counters_init(){
Glaxys_finest1 0:66a265dc3146 303 ch1.rise(&start_counter_1);
Glaxys_finest1 0:66a265dc3146 304 ch2.rise(&start_counter_2);
Glaxys_finest1 0:66a265dc3146 305 ch3.rise(&start_counter_3);
Glaxys_finest1 0:66a265dc3146 306 ch4.rise(&start_counter_4);
Glaxys_finest1 0:66a265dc3146 307 ch5.rise(&start_counter_5);
Glaxys_finest1 0:66a265dc3146 308 ch6.rise(&start_counter_6);
Glaxys_finest1 0:66a265dc3146 309 ch1.fall(&stop_counter_1);
Glaxys_finest1 0:66a265dc3146 310 ch2.fall(&stop_counter_2);
Glaxys_finest1 0:66a265dc3146 311 ch3.fall(&stop_counter_3);
Glaxys_finest1 0:66a265dc3146 312 ch4.fall(&stop_counter_4);
Glaxys_finest1 0:66a265dc3146 313 ch5.fall(&stop_counter_5);
Glaxys_finest1 0:66a265dc3146 314 ch6.fall(&stop_counter_6);
Glaxys_finest1 0:66a265dc3146 315 }
Glaxys_finest1 0:66a265dc3146 316
Glaxys_finest1 0:66a265dc3146 317
Glaxys_finest1 0:66a265dc3146 318 //////////////////////////////***********************************LIDAR********************************************////////////////
Glaxys_finest1 0:66a265dc3146 319 void dist_x() ///////////*******use this function to get distance it has object of rawserial as argument*******////////////
Glaxys_finest1 0:66a265dc3146 320 { //int distance;//actual distance measurements of LiDAR
Glaxys_finest1 0:66a265dc3146 321 int strength;//signal strength of LiDAR
Glaxys_finest1 0:66a265dc3146 322 int check;//save check value
Glaxys_finest1 0:66a265dc3146 323 int i;
Glaxys_finest1 0:66a265dc3146 324 int uartx[9];//save data measured by LiDAR
Glaxys_finest1 0:66a265dc3146 325
Glaxys_finest1 0:66a265dc3146 326 if (rawx.readable())//check if serial port has data input
Glaxys_finest1 0:66a265dc3146 327 {
Glaxys_finest1 0:66a265dc3146 328 if(rawx.getc()==0x59)//assess data package frame header 0x59
Glaxys_finest1 0:66a265dc3146 329 {
Glaxys_finest1 0:66a265dc3146 330 uartx[0]=0x59;
Glaxys_finest1 0:66a265dc3146 331 if(rawx.getc()==0x59)//assess data package frame header 0x59
Glaxys_finest1 0:66a265dc3146 332 {
Glaxys_finest1 0:66a265dc3146 333 uartx[1]=0x59;
Glaxys_finest1 0:66a265dc3146 334 for(i=2;i<9;i++)//save data in array
Glaxys_finest1 0:66a265dc3146 335 {
Glaxys_finest1 0:66a265dc3146 336 uartx[i]=rawx.getc();
Glaxys_finest1 0:66a265dc3146 337 }
Glaxys_finest1 0:66a265dc3146 338 check=uartx[0]+uartx[1]+uartx[2]+uartx[3]+uartx[4]+uartx[5]+uartx[6]+uartx[7];
Glaxys_finest1 0:66a265dc3146 339 if(uartx[8]==(check&0xff))//verify the received data as per protocol
Glaxys_finest1 0:66a265dc3146 340 {
Glaxys_finest1 0:66a265dc3146 341
Glaxys_finest1 0:66a265dc3146 342
Glaxys_finest1 0:66a265dc3146 343 strength=uartx[4]+uartx[5]*256;//calculate signal strength value
Glaxys_finest1 0:66a265dc3146 344 distx=uartx[2]+uartx[3]*256;//calculate distance value in cm
Glaxys_finest1 0:66a265dc3146 345 //velocityx = distx - distxo; ////velocity in meters
Glaxys_finest1 0:66a265dc3146 346 //distxo = distx;
Glaxys_finest1 0:66a265dc3146 347 }
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 void dist_y() ///////////*******use this function to get distance it has object of rawserial as argument*******////////////
Glaxys_finest1 0:66a265dc3146 354 {
Glaxys_finest1 0:66a265dc3146 355 int strength;//signal strength of LiDAR
Glaxys_finest1 0:66a265dc3146 356 int check;//save check value
Glaxys_finest1 0:66a265dc3146 357 int i;
Glaxys_finest1 0:66a265dc3146 358 int uarty[9];//save data measured by LiDAR
Glaxys_finest1 0:66a265dc3146 359
Glaxys_finest1 0:66a265dc3146 360 if (rawy.readable())//check if serial port has data input
Glaxys_finest1 0:66a265dc3146 361 {
Glaxys_finest1 0:66a265dc3146 362 if(rawy.getc()==0x59)//assess data package frame header 0x59
Glaxys_finest1 0:66a265dc3146 363 {
Glaxys_finest1 0:66a265dc3146 364 uarty[0]=0x59;
Glaxys_finest1 0:66a265dc3146 365 if(rawy.getc()==0x59)//assess data package frame header 0x59
Glaxys_finest1 0:66a265dc3146 366 {
Glaxys_finest1 0:66a265dc3146 367 uarty[1]=0x59;
Glaxys_finest1 0:66a265dc3146 368 for(i=2;i<9;i++)//save data in array
Glaxys_finest1 0:66a265dc3146 369 {
Glaxys_finest1 0:66a265dc3146 370 uarty[i]=rawy.getc();
Glaxys_finest1 0:66a265dc3146 371 }
Glaxys_finest1 0:66a265dc3146 372 check=uarty[0]+uarty[1]+uarty[2]+uarty[3]+uarty[4]+uarty[5]+uarty[6]+uarty[7];
Glaxys_finest1 0:66a265dc3146 373 if(uarty[8]==(check&0xff))//verify the received data as per protocol
Glaxys_finest1 0:66a265dc3146 374 {
Glaxys_finest1 0:66a265dc3146 375
Glaxys_finest1 0:66a265dc3146 376 strength=uarty[4]+uarty[5]*256;//calculate signal strength value
Glaxys_finest1 0:66a265dc3146 377 disty=uarty[2]+uarty[3]*256;//calculate distance value
Glaxys_finest1 0:66a265dc3146 378 //velocityy = disty - distyo; ////velocity in meters
Glaxys_finest1 0:66a265dc3146 379 //distyo = disty;
Glaxys_finest1 0:66a265dc3146 380 }
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 void dist_z() ///////////*******use this function to get distance it has object of rawserial as argument*******////////////
Glaxys_finest1 0:66a265dc3146 388 {
Glaxys_finest1 0:66a265dc3146 389 int strength;//signal strength of LiDAR
Glaxys_finest1 0:66a265dc3146 390 int check;//save check value
Glaxys_finest1 0:66a265dc3146 391 int i;
Glaxys_finest1 0:66a265dc3146 392 int uartz[9];//save data measured by LiDAR
Glaxys_finest1 0:66a265dc3146 393
Glaxys_finest1 0:66a265dc3146 394 if (rawz.readable())//check if serial port has data input
Glaxys_finest1 0:66a265dc3146 395 {
Glaxys_finest1 0:66a265dc3146 396 if(rawz.getc()==0x59)//assess data package frame header 0x59
Glaxys_finest1 0:66a265dc3146 397 {
Glaxys_finest1 0:66a265dc3146 398 uartz[0]=0x59;
Glaxys_finest1 0:66a265dc3146 399 if(rawz.getc()==0x59)//assess data package frame header 0x59
Glaxys_finest1 0:66a265dc3146 400 {
Glaxys_finest1 0:66a265dc3146 401 uartz[1]=0x59;
Glaxys_finest1 0:66a265dc3146 402 for(i=2;i<9;i++)//save data in array
Glaxys_finest1 0:66a265dc3146 403 {
Glaxys_finest1 0:66a265dc3146 404 uartz[i]=rawz.getc();
Glaxys_finest1 0:66a265dc3146 405 }
Glaxys_finest1 0:66a265dc3146 406 check=uartz[0]+uartz[1]+uartz[2]+uartz[3]+uartz[4]+uartz[5]+uartz[6]+uartz[7];
Glaxys_finest1 0:66a265dc3146 407 if(uartz[8]==(check&0xff))//verify the received data as per protocol
Glaxys_finest1 0:66a265dc3146 408 {
Glaxys_finest1 0:66a265dc3146 409 strength=uartz[4]+uartz[5]*256;//calculate signal strength value
Glaxys_finest1 0:66a265dc3146 410 distz=uartz[2]+uartz[3]*256;//calculate distance value
Glaxys_finest1 0:66a265dc3146 411 //velocityz = distz - distzo; ////velocity in meters
Glaxys_finest1 0:66a265dc3146 412 //distzo = distz;
Glaxys_finest1 0:66a265dc3146 413 }
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 //////////////////////////////////**********velocity**************//////////////////////////////////////
Glaxys_finest1 0:66a265dc3146 420
Glaxys_finest1 0:66a265dc3146 421
Glaxys_finest1 0:66a265dc3146 422 /////////////////////////////////***********************mpu****************************///////////////////////////////
Glaxys_finest1 0:66a265dc3146 423
Glaxys_finest1 0:66a265dc3146 424 void mpu_init()
Glaxys_finest1 0:66a265dc3146 425 {
Glaxys_finest1 0:66a265dc3146 426 i2c.frequency(100000);
Glaxys_finest1 0:66a265dc3146 427 char data_write[2];
Glaxys_finest1 0:66a265dc3146 428
Glaxys_finest1 0:66a265dc3146 429 data_write[0] = MPU6050_PWR_MGMT_1;
Glaxys_finest1 0:66a265dc3146 430 data_write[1] = 0x00;
Glaxys_finest1 0:66a265dc3146 431
Glaxys_finest1 0:66a265dc3146 432 int status = i2c.write(MPU6050_ADDR, data_write, 2, 0);
Glaxys_finest1 0:66a265dc3146 433
Glaxys_finest1 0:66a265dc3146 434 if (status != 0)
Glaxys_finest1 0:66a265dc3146 435 { // Error
Glaxys_finest1 0:66a265dc3146 436 while (1)
Glaxys_finest1 0:66a265dc3146 437 {
Glaxys_finest1 0:66a265dc3146 438 myled = !myled;
Glaxys_finest1 0:66a265dc3146 439 wait(0.2);
Glaxys_finest1 0:66a265dc3146 440 }
Glaxys_finest1 0:66a265dc3146 441 }
Glaxys_finest1 0:66a265dc3146 442 data_write[0] = USER_CTRL;
Glaxys_finest1 0:66a265dc3146 443 data_write[1] = I2C_MST_EN;
Glaxys_finest1 0:66a265dc3146 444 i2c.write(MPU6050_ADDR, data_write, 2, 0);
Glaxys_finest1 0:66a265dc3146 445
Glaxys_finest1 0:66a265dc3146 446 data_write[0] = 0x37;//INT_PIN_CFG;
Glaxys_finest1 0:66a265dc3146 447 data_write[1] = 0x02;
Glaxys_finest1 0:66a265dc3146 448 i2c.write(MPU6050_ADDR, data_write, 2, 0);
Glaxys_finest1 0:66a265dc3146 449
Glaxys_finest1 0:66a265dc3146 450 data_write[0] = MPU6050_SMPLRT_DIV;
Glaxys_finest1 0:66a265dc3146 451 data_write[1] = 0x07;
Glaxys_finest1 0:66a265dc3146 452 i2c.write(MPU6050_ADDR, data_write, 2, 0);
Glaxys_finest1 0:66a265dc3146 453
Glaxys_finest1 0:66a265dc3146 454 data_write[0] = MPU6050_ACCEL_CONFIG;
Glaxys_finest1 0:66a265dc3146 455 data_write[1] = 0x00;
Glaxys_finest1 0:66a265dc3146 456 i2c.write(MPU6050_ADDR, data_write, 2, 0);
Glaxys_finest1 0:66a265dc3146 457 data_write[0] = 0x1d;
Glaxys_finest1 0:66a265dc3146 458 data_write[1] = 0x06;
Glaxys_finest1 0:66a265dc3146 459 i2c.write(MPU6050_ADDR, data_write, 2, 0);
Glaxys_finest1 0:66a265dc3146 460 data_write[0] = MPU6050_CONFIG;
Glaxys_finest1 0:66a265dc3146 461 data_write[1] = 0x00;
Glaxys_finest1 0:66a265dc3146 462 i2c.write(MPU6050_ADDR, data_write, 2, 0);
Glaxys_finest1 0:66a265dc3146 463
Glaxys_finest1 0:66a265dc3146 464 data_write[0] = MPU6050_GYRO_CONFIG;
Glaxys_finest1 0:66a265dc3146 465 data_write[1] = 0x00;
Glaxys_finest1 0:66a265dc3146 466 i2c.write(MPU6050_ADDR, data_write, 2, 0);
Glaxys_finest1 0:66a265dc3146 467 wait(0.2);
Glaxys_finest1 0:66a265dc3146 468 }
Glaxys_finest1 0:66a265dc3146 469
Glaxys_finest1 0:66a265dc3146 470 void mpu_read(){
Glaxys_finest1 0:66a265dc3146 471 char Rec_Data[14];
Glaxys_finest1 0:66a265dc3146 472 int16_t Accel_X_RAW = 0;
Glaxys_finest1 0:66a265dc3146 473 int16_t Accel_Y_RAW = 0;
Glaxys_finest1 0:66a265dc3146 474 int16_t Accel_Z_RAW = 0;
Glaxys_finest1 0:66a265dc3146 475 int16_t Temp_raw = 0;
Glaxys_finest1 0:66a265dc3146 476 int16_t Gyro_X_RAW = 0;
Glaxys_finest1 0:66a265dc3146 477 int16_t Gyro_Y_RAW = 0;
Glaxys_finest1 0:66a265dc3146 478 int16_t Gyro_Z_RAW = 0;
Glaxys_finest1 0:66a265dc3146 479 char data_write[1];
Glaxys_finest1 0:66a265dc3146 480
Glaxys_finest1 0:66a265dc3146 481 // Read 1 BYTES of data starting from ACCEL_XOUT_H register
Glaxys_finest1 0:66a265dc3146 482 data_write[0] = ACCEL_XOUT_H_REG;
Glaxys_finest1 0:66a265dc3146 483 i2c.write(MPU6050_ADDR, data_write, 1, 1); // no stop
Glaxys_finest1 0:66a265dc3146 484 i2c.read(MPU6050_ADDR, Rec_Data, 14);
Glaxys_finest1 0:66a265dc3146 485 Accel_X_RAW = (int16_t)(Rec_Data[0] << 8 | Rec_Data [1]);
Glaxys_finest1 0:66a265dc3146 486 Accel_Y_RAW = (int16_t)(Rec_Data[2] << 8 | Rec_Data [3]);
Glaxys_finest1 0:66a265dc3146 487 Accel_Z_RAW = (int16_t)(Rec_Data[4] << 8 | Rec_Data [5]);
Glaxys_finest1 0:66a265dc3146 488
Glaxys_finest1 0:66a265dc3146 489 Temp_raw = (int16_t)(Rec_Data[6] << 8 | Rec_Data [7]);
Glaxys_finest1 0:66a265dc3146 490
Glaxys_finest1 0:66a265dc3146 491 Gyro_X_RAW = (int16_t)(Rec_Data[8] << 8 | Rec_Data [9]);
Glaxys_finest1 0:66a265dc3146 492 Gyro_Y_RAW = (int16_t)(Rec_Data[10] << 8 | Rec_Data [11]);
Glaxys_finest1 0:66a265dc3146 493 Gyro_Z_RAW = (int16_t)(Rec_Data[12] << 8 | Rec_Data [13]);
Glaxys_finest1 0:66a265dc3146 494 /*** convert the RAW values into acceleration in 'g'
Glaxys_finest1 0:66a265dc3146 495 we have to divide according to the Full scale value set in FS_SEL
Glaxys_finest1 0:66a265dc3146 496 I have configured FS_SEL = 0. So I am dividing by 16384.0
Glaxys_finest1 0:66a265dc3146 497 for more details check ACCEL_CONFIG Register ****/
Glaxys_finest1 0:66a265dc3146 498
Glaxys_finest1 0:66a265dc3146 499 Ax = Accel_X_RAW/16384.0;
Glaxys_finest1 0:66a265dc3146 500 Ay = Accel_Y_RAW/16384.0;
Glaxys_finest1 0:66a265dc3146 501 Az = Accel_Z_RAW/16384.0;
Glaxys_finest1 0:66a265dc3146 502
Glaxys_finest1 0:66a265dc3146 503 Gx = Gyro_X_RAW/131.0;
Glaxys_finest1 0:66a265dc3146 504 Gy = Gyro_Y_RAW/131.0;
Glaxys_finest1 0:66a265dc3146 505 Gz = Gyro_Z_RAW/131.0;
Glaxys_finest1 0:66a265dc3146 506
Glaxys_finest1 0:66a265dc3146 507 }
Glaxys_finest1 0:66a265dc3146 508
Glaxys_finest1 0:66a265dc3146 509
Glaxys_finest1 0:66a265dc3146 510 void get_angle(){
Glaxys_finest1 0:66a265dc3146 511
Glaxys_finest1 0:66a265dc3146 512 mpu_read();
Glaxys_finest1 0:66a265dc3146 513 dt2=timer2.read_ms(); ////////****very imp self note check the order of dt1 and dt2*********///////////
Glaxys_finest1 0:66a265dc3146 514 dt=dt2-dt1;
Glaxys_finest1 0:66a265dc3146 515 dt=dt/1000;
Glaxys_finest1 0:66a265dc3146 516
Glaxys_finest1 0:66a265dc3146 517 roll_acc = (atan2((Ay),sqrt(pow((Ax),2) + pow((Az),2))))*(90*7/11);
Glaxys_finest1 0:66a265dc3146 518 pitch_acc = ((atan2((-Ax),sqrt(pow((Ay),2) + pow((Az),2))))*(90*7/11));
Glaxys_finest1 0:66a265dc3146 519
Glaxys_finest1 0:66a265dc3146 520 /* COMPLIMENTARY FILTER*/
Glaxys_finest1 0:66a265dc3146 521 pitch = Alfa_comp*(pitch + Gy*dt) + (1-Alfa_comp)*pitch_acc ;
Glaxys_finest1 0:66a265dc3146 522
Glaxys_finest1 0:66a265dc3146 523 roll = Alfa_comp*(roll + Gx*dt) + (1-Alfa_comp)*roll_acc ;
Glaxys_finest1 0:66a265dc3146 524 if(abs(Gz)>1)
Glaxys_finest1 0:66a265dc3146 525 yaw += Gz*dt;
Glaxys_finest1 0:66a265dc3146 526 dt1=timer2.read_ms();
Glaxys_finest1 0:66a265dc3146 527
Glaxys_finest1 0:66a265dc3146 528 }
Glaxys_finest1 0:66a265dc3146 529 void mpu_calibrate(void){
Glaxys_finest1 0:66a265dc3146 530 int i;
Glaxys_finest1 0:66a265dc3146 531 for(i=0;i<=3000;i++)
Glaxys_finest1 0:66a265dc3146 532 {
Glaxys_finest1 0:66a265dc3146 533 //dt2=timer2.read_ms();
Glaxys_finest1 0:66a265dc3146 534 if(i>0){
Glaxys_finest1 0:66a265dc3146 535 get_angle();}
Glaxys_finest1 0:66a265dc3146 536
Glaxys_finest1 0:66a265dc3146 537 // dt1=timer2.read_ms();
Glaxys_finest1 0:66a265dc3146 538 //dt=dt1-dt2;
Glaxys_finest1 0:66a265dc3146 539
Glaxys_finest1 0:66a265dc3146 540 avgx+=roll;
Glaxys_finest1 0:66a265dc3146 541 avgy+=pitch;
Glaxys_finest1 0:66a265dc3146 542 //avgz+=yaw;
Glaxys_finest1 0:66a265dc3146 543 }
Glaxys_finest1 0:66a265dc3146 544 avgx=avgx/3000;
Glaxys_finest1 0:66a265dc3146 545 avgy=avgy/3000;
Glaxys_finest1 0:66a265dc3146 546 // avgz=avgz/2000;
Glaxys_finest1 0:66a265dc3146 547 }
Glaxys_finest1 0:66a265dc3146 548 /*void Mag_init(){
Glaxys_finest1 0:66a265dc3146 549 char raw[3];
Glaxys_finest1 0:66a265dc3146 550 char data_write[2];
Glaxys_finest1 0:66a265dc3146 551
Glaxys_finest1 0:66a265dc3146 552 ///////////////////////////////////////// Power down magnetometer///////////////////////////////////////////////////////
Glaxys_finest1 0:66a265dc3146 553 data_write[0] = AK8963_CNTL;
Glaxys_finest1 0:66a265dc3146 554 data_write[1] = 0x00;
Glaxys_finest1 0:66a265dc3146 555
Glaxys_finest1 0:66a265dc3146 556 int status = i2c.write(AK8963_ADDRESS, data_write, 2, 0);
Glaxys_finest1 0:66a265dc3146 557 if (status != 0)
Glaxys_finest1 0:66a265dc3146 558 { // Error
Glaxys_finest1 0:66a265dc3146 559 while (1)
Glaxys_finest1 0:66a265dc3146 560 {
Glaxys_finest1 0:66a265dc3146 561 myled = !myled;
Glaxys_finest1 0:66a265dc3146 562 wait(0.2);
Glaxys_finest1 0:66a265dc3146 563 }
Glaxys_finest1 0:66a265dc3146 564 }
Glaxys_finest1 0:66a265dc3146 565
Glaxys_finest1 0:66a265dc3146 566 wait(0.01);
Glaxys_finest1 0:66a265dc3146 567 data_write[0] = 0x6B; //////////////////rest magnetometer
Glaxys_finest1 0:66a265dc3146 568 data_write[1] = 0x80;
Glaxys_finest1 0:66a265dc3146 569
Glaxys_finest1 0:66a265dc3146 570 i2c.write(MPU6050_ADDR, data_write, 2, 0);
Glaxys_finest1 0:66a265dc3146 571
Glaxys_finest1 0:66a265dc3146 572
Glaxys_finest1 0:66a265dc3146 573 wait(0.01);
Glaxys_finest1 0:66a265dc3146 574 ///////////////////////////////////////// Enter Fuse ROM access mode///////////////////////////////////////////////////
Glaxys_finest1 0:66a265dc3146 575 data_write[0] = AK8963_CNTL;
Glaxys_finest1 0:66a265dc3146 576 data_write[1] = 0x0F;
Glaxys_finest1 0:66a265dc3146 577 i2c.write(AK8963_ADDRESS, data_write, 2, 0);
Glaxys_finest1 0:66a265dc3146 578
Glaxys_finest1 0:66a265dc3146 579 wait(0.01);
Glaxys_finest1 0:66a265dc3146 580 ///////////////////////////////////////// Read the x-, y-, and z-axis calibration values//////////////////////////////
Glaxys_finest1 0:66a265dc3146 581 data_write[0] = AK8963_ASAX;
Glaxys_finest1 0:66a265dc3146 582 i2c.write(AK8963_ADDRESS, data_write, 1, 0);
Glaxys_finest1 0:66a265dc3146 583 i2c.read(AK8963_ADDRESS, raw, 3);
Glaxys_finest1 0:66a265dc3146 584 // Return x-axis sensitivity adjustment values, etc.
Glaxys_finest1 0:66a265dc3146 585 H_senx = (float)(raw[0] - 128)/256.0f + 1.0f;
Glaxys_finest1 0:66a265dc3146 586 H_seny = (float)(raw[1] - 128)/256.0f + 1.0f;
Glaxys_finest1 0:66a265dc3146 587 H_senz = (float)(raw[2] - 128)/256.0f + 1.0f;
Glaxys_finest1 0:66a265dc3146 588
Glaxys_finest1 0:66a265dc3146 589 data_write[0] = AK8963_CNTL;
Glaxys_finest1 0:66a265dc3146 590 data_write[1] = 0x16;
Glaxys_finest1 0:66a265dc3146 591 i2c.write(AK8963_ADDRESS, data_write, 2, 0);
Glaxys_finest1 0:66a265dc3146 592 wait(0.01);
Glaxys_finest1 0:66a265dc3146 593 data_write[0] = 0x0B;
Glaxys_finest1 0:66a265dc3146 594 data_write[1] = 0x01;
Glaxys_finest1 0:66a265dc3146 595 i2c.write(AK8963_ADDRESS, data_write, 2, 0);
Glaxys_finest1 0:66a265dc3146 596 wait(0.01);
Glaxys_finest1 0:66a265dc3146 597 // Configure the magnetometer for continuous read and highest resolution
Glaxys_finest1 0:66a265dc3146 598 // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register,
Glaxys_finest1 0:66a265dc3146 599 // 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 600
Glaxys_finest1 0:66a265dc3146 601 }
Glaxys_finest1 0:66a265dc3146 602 void Mag_read(){
Glaxys_finest1 0:66a265dc3146 603
Glaxys_finest1 0:66a265dc3146 604 char data_write[1];
Glaxys_finest1 0:66a265dc3146 605 char x[1];
Glaxys_finest1 0:66a265dc3146 606 char rawData[7];
Glaxys_finest1 0:66a265dc3146 607 /////////////read name///////////////////////////////////
Glaxys_finest1 0:66a265dc3146 608 data_write[0] = WHO_AM_I_AK8963;
Glaxys_finest1 0:66a265dc3146 609 i2c.write(AK8963_ADDRESS, data_write, 1, 0);
Glaxys_finest1 0:66a265dc3146 610 i2c.read(AK8963_ADDRESS, name, 1);
Glaxys_finest1 0:66a265dc3146 611
Glaxys_finest1 0:66a265dc3146 612 //////////////////////////////////////
Glaxys_finest1 0:66a265dc3146 613 data_write[0] = AK8963_ST1;
Glaxys_finest1 0:66a265dc3146 614 i2c.write(AK8963_ADDRESS, data_write, 1, 0);
Glaxys_finest1 0:66a265dc3146 615 i2c.read(AK8963_ADDRESS, x, 1);
Glaxys_finest1 0:66a265dc3146 616
Glaxys_finest1 0:66a265dc3146 617 if(x[0] & 0x01) { // wait for magnetometer data ready bit to be set
Glaxys_finest1 0:66a265dc3146 618 data_write[0] = AK8963_XOUT_L;// Read the six raw data and ST2 registers sequentially into data array
Glaxys_finest1 0:66a265dc3146 619 i2c.write(AK8963_ADDRESS, data_write, 1, 1);
Glaxys_finest1 0:66a265dc3146 620 i2c.read(AK8963_ADDRESS, rawData, 7);
Glaxys_finest1 0:66a265dc3146 621
Glaxys_finest1 0:66a265dc3146 622 uint8_t c = rawData[6]; // End data read by reading ST2 register
Glaxys_finest1 0:66a265dc3146 623 if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data
Glaxys_finest1 0:66a265dc3146 624 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 625 Mag_Y_RAW = (int16_t)((rawData[3] << 8) | rawData[2]) ; // Data stored as little Endian
Glaxys_finest1 0:66a265dc3146 626 Mag_Z_RAW = (int16_t)((rawData[5] << 8) | rawData[4]) ;
Glaxys_finest1 0:66a265dc3146 627 }
Glaxys_finest1 0:66a265dc3146 628 }
Glaxys_finest1 0:66a265dc3146 629 Mx = Mag_X_RAW*H_senx;
Glaxys_finest1 0:66a265dc3146 630 My = Mag_Y_RAW*H_seny;
Glaxys_finest1 0:66a265dc3146 631 Mz = Mag_Z_RAW*H_senz;
Glaxys_finest1 0:66a265dc3146 632 }*/