ENSMM / Mbed 2 deprecated BMC

Programme complet de bâton de marche sauf capteur cardiaque

Dependencies:   mbed SimpleBLE X_NUCLEO_IDB0XA1 SDFileSystem MBed_Adafruit-GPS-Library Arduino USBDevice

main.cpp@0:6e330c197193, 2020-01-27 (annotated)

Committer:
zmoutaou
Date:
Mon Jan 27 12:04:30 2020 +0000
Revision:
0:6e330c197193
BMC

Who changed what in which revision?

UserRevisionLine numberNew contents of line
zmoutaou 0:6e330c197193 1 //Includes
zmoutaou 0:6e330c197193 2
zmoutaou 0:6e330c197193 3 //#include "mbed.h"//déja défini dans une bibliotheque
zmoutaou 0:6e330c197193 4 #include "SimpleBLE.h"
zmoutaou 0:6e330c197193 5 #include "USBSerial.h"
zmoutaou 0:6e330c197193 6 #include "SDFileSystem.h"
zmoutaou 0:6e330c197193 7 #include "Adafruit_SSD1306.h"
zmoutaou 0:6e330c197193 8 #include "Arduino.h"
zmoutaou 0:6e330c197193 9 #include "MBed_Adafruit_GPS.h"
zmoutaou 0:6e330c197193 10 #include "LSM9DS1_registre.h"
zmoutaou 0:6e330c197193 11
zmoutaou 0:6e330c197193 12
zmoutaou 0:6e330c197193 13
zmoutaou 0:6e330c197193 14
zmoutaou 0:6e330c197193 15 Timer dt ;
zmoutaou 0:6e330c197193 16 int t_avant = 0 ;
zmoutaou 0:6e330c197193 17 int t_m = 0 ;
zmoutaou 0:6e330c197193 18 int t_d = 0;
zmoutaou 0:6e330c197193 19 int t_apres = 0;
zmoutaou 0:6e330c197193 20
zmoutaou 0:6e330c197193 21
zmoutaou 0:6e330c197193 22
zmoutaou 0:6e330c197193 23
zmoutaou 0:6e330c197193 24 I2C i2c_m(PB_9, PB_8);
zmoutaou 0:6e330c197193 25 unsigned char nCRC; // calculated check sum to ensure PROM integrity
zmoutaou 0:6e330c197193 26 double dT, OFFSET, SENS, T2, OFFSET2, SENS2; // First order and second order corrections for raw S5637 temperature and pressure data
zmoutaou 0:6e330c197193 27 int16_t accelCount[3], gyroCount[3], magCount[3]; // Stores the 16-bit signed accelerometer, gyro, and mag sensor output
zmoutaou 0:6e330c197193 28 float gyroBias1[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}, magBias[3] = {0, 0, 0}; // Bias corrections for gyro, accelerometer, and magnetometer
zmoutaou 0:6e330c197193 29 int16_t tempCount; // temperature raw count output
zmoutaou 0:6e330c197193 30 float temperature; // Stores the LSM9DS1gyro internal chip temperature in degrees Celsius
zmoutaou 0:6e330c197193 31 double Temperature, Pressure; // stores MS5611 pressures sensor pressure and temperature
zmoutaou 0:6e330c197193 32 // global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System)
zmoutaou 0:6e330c197193 33 float GyroMeasError = PI * (40.0f / 180.0f); // gyroscope measurement error in rads/s (start at 40 deg/s)
zmoutaou 0:6e330c197193 34 float GyroMeasDrift = PI * (0.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
zmoutaou 0:6e330c197193 35 // There is a tradeoff in the beta parameter between accuracy and response speed.
zmoutaou 0:6e330c197193 36 // In the original Madgwick study, beta of 0.041 (corresponding to GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy.
zmoutaou 0:6e330c197193 37 // However, with this value, the LSM9SD0 response time is about 10 seconds to a stable initial quaternion.
zmoutaou 0:6e330c197193 38 // Subsequent changes also require a longish lag time to a stable output, not fast enough for a quadcopter or robot car!
zmoutaou 0:6e330c197193 39 // By increasing beta (GyroMeasError) by about a factor of fifteen, the response time constant is reduced to ~2 sec
zmoutaou 0:6e330c197193 40 // I haven't noticed any reduction in solution accuracy. This is essentially the I coefficient in a PID control sense;
zmoutaou 0:6e330c197193 41 // the bigger the feedback coefficient, the faster the solution converges, usually at the expense of accuracy.
zmoutaou 0:6e330c197193 42 // In any case, this is the free parameter in the Madgwick filtering and fusion scheme.
zmoutaou 0:6e330c197193 43 float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta
zmoutaou 0:6e330c197193 44 float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value
zmoutaou 0:6e330c197193 45 #define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral
zmoutaou 0:6e330c197193 46 #define Ki 0.0f
zmoutaou 0:6e330c197193 47
zmoutaou 0:6e330c197193 48 uint32_t delt_t = 0, count = 0, sumCount = 0; // used to control display output rate
zmoutaou 0:6e330c197193 49 float pitch, yaw, roll;
zmoutaou 0:6e330c197193 50 float deltat = 0.0f, sum = 0.0f; // integration interval for both filter schemes
zmoutaou 0:6e330c197193 51 uint32_t Now = 0; // used to calculate integration interval
zmoutaou 0:6e330c197193 52
zmoutaou 0:6e330c197193 53 float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values
zmoutaou 0:6e330c197193 54 float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion
zmoutaou 0:6e330c197193 55 float eInt[3] = {0.0f, 0.0f, 0.0f}; // vector to hold integral error for Mahony meth
zmoutaou 0:6e330c197193 56
zmoutaou 0:6e330c197193 57 // Specify sensor full scale
zmoutaou 0:6e330c197193 58 uint8_t Gscale = GFS_245DPS; // gyro full scale
zmoutaou 0:6e330c197193 59 uint8_t Godr = GODR_238Hz; // gyro data sample rate
zmoutaou 0:6e330c197193 60 uint8_t Gbw = GBW_med; // gyro data bandwidth
zmoutaou 0:6e330c197193 61 uint8_t Ascale = AFS_2G; // accel full scale
zmoutaou 0:6e330c197193 62 uint8_t Aodr = AODR_238Hz; // accel data sample rate
zmoutaou 0:6e330c197193 63 uint8_t Abw = ABW_50Hz; // accel data bandwidth
zmoutaou 0:6e330c197193 64 uint8_t Mscale = MFS_4G; // mag full scale
zmoutaou 0:6e330c197193 65 uint8_t Modr = MODR_10Hz; // mag data sample rate
zmoutaou 0:6e330c197193 66 uint8_t Mmode = MMode_HighPerformance; // magnetometer operation mode
zmoutaou 0:6e330c197193 67 float aRes, gRes, mRes; // scale resolutions per LSB for the sensors
zmoutaou 0:6e330c197193 68
zmoutaou 0:6e330c197193 69
zmoutaou 0:6e330c197193 70 void getMres();
zmoutaou 0:6e330c197193 71 void getGres();
zmoutaou 0:6e330c197193 72 void getAres();
zmoutaou 0:6e330c197193 73 void writeByte(uint8_t address, uint8_t subAddress, uint8_t data);
zmoutaou 0:6e330c197193 74 uint8_t readByte(uint8_t address, uint8_t subAddress);
zmoutaou 0:6e330c197193 75 void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest);
zmoutaou 0:6e330c197193 76 void accelgyrocalLSM9DS1(float * dest1, float * dest2);
zmoutaou 0:6e330c197193 77 void magcalLSM9DS1(float * dest1);
zmoutaou 0:6e330c197193 78 void readAccelData(int16_t * destination);
zmoutaou 0:6e330c197193 79 void readGyroData(int16_t * destination);
zmoutaou 0:6e330c197193 80 void readMagData(int16_t * destination);
zmoutaou 0:6e330c197193 81 void initLSM9DS1();
zmoutaou 0:6e330c197193 82 void selftestLSM9DS1();
zmoutaou 0:6e330c197193 83
zmoutaou 0:6e330c197193 84
zmoutaou 0:6e330c197193 85 Serial * gps_Serial;
zmoutaou 0:6e330c197193 86 SDFileSystem sd(PA_7, PA_6, PA_5, PB_6, "sd");
zmoutaou 0:6e330c197193 87 SimpleBLE ble("ObCP_Baton ");
zmoutaou 0:6e330c197193 88 FILE *fp_ble= fopen("/sd/valeur_ble.txt", "w");
zmoutaou 0:6e330c197193 89 FILE *fp= fopen("/sd/valeur_force.txt", "w");
zmoutaou 0:6e330c197193 90 I2C i2c(PB_9, PB_8);
zmoutaou 0:6e330c197193 91 Adafruit_SSD1306_I2c oled(i2c,PC_13);
zmoutaou 0:6e330c197193 92 USBSerial pc(0x1f00, 0x2012, 0x0001, false);
zmoutaou 0:6e330c197193 93
zmoutaou 0:6e330c197193 94 AnalogIn tension_A502(A0);
zmoutaou 0:6e330c197193 95 DigitalOut myledr(PB_5);
zmoutaou 0:6e330c197193 96 DigitalOut myledg(PB_3);
zmoutaou 0:6e330c197193 97 DigitalOut myledb(PA_10);
zmoutaou 0:6e330c197193 98 InterruptIn button1(PB_4);
zmoutaou 0:6e330c197193 99 InterruptIn button2(PB_10);
zmoutaou 0:6e330c197193 100
zmoutaou 0:6e330c197193 101
zmoutaou 0:6e330c197193 102 float lat_dd, lon_dd, lat_1, lon_1, lat_2, lon_2, distance = 0;
zmoutaou 0:6e330c197193 103 float sum_distance = 0;
zmoutaou 0:6e330c197193 104 float steps = 0;
zmoutaou 0:6e330c197193 105 double avg_speed = 0;
zmoutaou 0:6e330c197193 106 float cal = 0;
zmoutaou 0:6e330c197193 107 int i_gps ;
zmoutaou 0:6e330c197193 108 float t =0;
zmoutaou 0:6e330c197193 109 float lastUpdate = 0;
zmoutaou 0:6e330c197193 110
zmoutaou 0:6e330c197193 111 int height=0;
zmoutaou 0:6e330c197193 112 bool interruption1= false;
zmoutaou 0:6e330c197193 113 char name[20];
zmoutaou 0:6e330c197193 114 int i=0 ;
zmoutaou 0:6e330c197193 115 int weight;
zmoutaou 0:6e330c197193 116 int age ;
zmoutaou 0:6e330c197193 117 char gender[1];
zmoutaou 0:6e330c197193 118 int max_heart_rate;
zmoutaou 0:6e330c197193 119 bool message_envoyee= false;
zmoutaou 0:6e330c197193 120
zmoutaou 0:6e330c197193 121
zmoutaou 0:6e330c197193 122 const int size_data = 50;
zmoutaou 0:6e330c197193 123 float force_measure[size_data];
zmoutaou 0:6e330c197193 124 float pression_measure[size_data];
zmoutaou 0:6e330c197193 125 float volts_measure[size_data];
zmoutaou 0:6e330c197193 126 int size_data_i= 0;
zmoutaou 0:6e330c197193 127 bool interruption2;
zmoutaou 0:6e330c197193 128 bool force_mesure = false;
zmoutaou 0:6e330c197193 129
zmoutaou 0:6e330c197193 130 void fct_name_text(uint8_t newState);
zmoutaou 0:6e330c197193 131 void fct_height_int(uint8_t newState);
zmoutaou 0:6e330c197193 132 void fct_weight_int(uint8_t newState);
zmoutaou 0:6e330c197193 133 void fct_age_int(uint8_t newState);
zmoutaou 0:6e330c197193 134 void fct_gender_text(uint8_t newState);
zmoutaou 0:6e330c197193 135 void fct_max_heart_rate_int(uint8_t newState);
zmoutaou 0:6e330c197193 136 void fct_fini(uint8_t newState);
zmoutaou 0:6e330c197193 137
zmoutaou 0:6e330c197193 138 void fct_interruption_ble();
zmoutaou 0:6e330c197193 139
zmoutaou 0:6e330c197193 140
zmoutaou 0:6e330c197193 141 int intToStr(int x, char str[], int d);
zmoutaou 0:6e330c197193 142 void ftoa(float n, char* res, int afterpoint);
zmoutaou 0:6e330c197193 143 float pow_nc(float n, int k);
zmoutaou 0:6e330c197193 144 void reverse(char* str, int len) ;
zmoutaou 0:6e330c197193 145
zmoutaou 0:6e330c197193 146 void fct_interruption_force(void);
zmoutaou 0:6e330c197193 147
zmoutaou 0:6e330c197193 148 void OLED_writeString(string str, int x, int y);
zmoutaou 0:6e330c197193 149 void OLED_drawSmile();
zmoutaou 0:6e330c197193 150
zmoutaou 0:6e330c197193 151
zmoutaou 0:6e330c197193 152
zmoutaou 0:6e330c197193 153 SimpleChar<uint8_t> name_text = ble.writeOnly_u8(0x8600, 0x8601, &fct_name_text);
zmoutaou 0:6e330c197193 154 SimpleChar<uint8_t> height_int = ble.writeOnly_u8(0x8600, 0x8602, &fct_height_int);
zmoutaou 0:6e330c197193 155 SimpleChar<uint8_t> weight_int= ble.writeOnly_u8(0x8600, 0x8603, &fct_weight_int);
zmoutaou 0:6e330c197193 156 SimpleChar<uint8_t> age_int= ble.writeOnly_u8(0x8600, 0x8604, &fct_age_int);
zmoutaou 0:6e330c197193 157 SimpleChar<uint8_t> gender_text= ble.writeOnly_u8(0x8600, 0x8605, &fct_gender_text);
zmoutaou 0:6e330c197193 158 SimpleChar<uint8_t> max_heart_rate_u8= ble.writeOnly_u8(0x8600, 0x8606, &fct_max_heart_rate_int);
zmoutaou 0:6e330c197193 159 SimpleChar<uint8_t> fini = ble.writeOnly_u8(0x8600, 0x8607, &fct_fini);
zmoutaou 0:6e330c197193 160
zmoutaou 0:6e330c197193 161 int main(int, char**)
zmoutaou 0:6e330c197193 162 {
zmoutaou 0:6e330c197193 163 bool erreur_imu = false;
zmoutaou 0:6e330c197193 164 ble.start();
zmoutaou 0:6e330c197193 165 gps_Serial = new Serial(PA_2,PA_3); //serial object for use w/ GPS
zmoutaou 0:6e330c197193 166 Adafruit_GPS myGPS(gps_Serial); //object of Adafruit's GPS class
zmoutaou 0:6e330c197193 167 char c; //when read via Adafruit_GPS::read(), the class returns single character stored here
zmoutaou 0:6e330c197193 168 Timer refresh_Timer; //sets up a timer for use in loop; how often do we print GPS info?
zmoutaou 0:6e330c197193 169 const int refresh_Time = 2000; //refresh time in ms
zmoutaou 0:6e330c197193 170 button1.rise(&fct_interruption_ble);
zmoutaou 0:6e330c197193 171 button2.rise(&fct_interruption_force);
zmoutaou 0:6e330c197193 172 uint8_t c_who_I_m = readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_WHO_AM_I);
zmoutaou 0:6e330c197193 173 uint8_t d_who_I_m = readByte(LSM9DS1M_ADDRESS, LSM9DS1M_WHO_AM_I); // Read WHO_AM_I register for LSM9DS1 magnetometer//pc.printf("LSM9DS1 magnetometer % d"); ////////pc.printf("I AM %x",d); ////////pc.printf(" I should be 0x3D\n");
zmoutaou 0:6e330c197193 174 if (c_who_I_m == 0x68 && d_who_I_m == 0x3D) {
zmoutaou 0:6e330c197193 175 // WHO_AM_I should always be 0x0E for the accel/gyro and 0x3C for the mag
zmoutaou 0:6e330c197193 176 getAres();////////pc.printf("accel sensitivity is %f .",1./(1000.*aRes)); //////////////pc.printf(" LSB/mg");
zmoutaou 0:6e330c197193 177 getGres();////////pc.printf("gyro sensitivity is %f ." ,1./(1000.*gRes)); //////////////pc.printf(" LSB/mdps");
zmoutaou 0:6e330c197193 178 getMres();////////pc.printf("mag sensitivity is %f .", 1./(1000.*mRes)); //////////////pc.printf(" LSB/mGauss");
zmoutaou 0:6e330c197193 179 selftestLSM9DS1(); ////////pc.printf("Perform gyro and accel self test"); // check function of gyro and accelerometer via self test
zmoutaou 0:6e330c197193 180 accelgyrocalLSM9DS1( gyroBias1, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers////////pc.printf("accel biases (mg) %f %f %f \n",1000.*accelBias[0],1000.*accelBias[1],1000.*accelBias[2]);////////pc.printf("gyro biases (dps) %f %f %f \n",gyroBias[0],gyroBias[1],gyroBias[2]);
zmoutaou 0:6e330c197193 181 magcalLSM9DS1(magBias);////////pc.printf("mag biases (mG) %f %f %f \n",1000.*magBias[0],1000.*magBias[1],1000.*magBias[2]);
zmoutaou 0:6e330c197193 182 wait(2); // add delay to see results before serial spew of data
zmoutaou 0:6e330c197193 183 initLSM9DS1(); ////////pc.printf("LSM9DS1 initialized for active data mode...."); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
zmoutaou 0:6e330c197193 184 erreur_imu = true;
zmoutaou 0:6e330c197193 185 }
zmoutaou 0:6e330c197193 186 myGPS.begin(9600);
zmoutaou 0:6e330c197193 187 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA); //these commands are defined in MBed_Adafruit_GPS.h; a link is provided there for command creation
zmoutaou 0:6e330c197193 188 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
zmoutaou 0:6e330c197193 189 myGPS.sendCommand(PGCMD_ANTENNA);
zmoutaou 0:6e330c197193 190 oled.clearDisplay();
zmoutaou 0:6e330c197193 191 OLED_writeString("Hello! ", 32, 16);
zmoutaou 0:6e330c197193 192 pc.printf("\n********************************Bonjour**************************************\n");
zmoutaou 0:6e330c197193 193 refresh_Timer.start();
zmoutaou 0:6e330c197193 194 while (1) {
zmoutaou 0:6e330c197193 195 while ( interruption1 == true) {
zmoutaou 0:6e330c197193 196 myledr =1 ;
zmoutaou 0:6e330c197193 197 if ( message_envoyee== true ) {
zmoutaou 0:6e330c197193 198 wait(1);
zmoutaou 0:6e330c197193 199 fprintf(fp,"\n********************************BLE**************************************\n");
zmoutaou 0:6e330c197193 200 fprintf(fp,"name: %s ; ",name);
zmoutaou 0:6e330c197193 201 fprintf(fp,"height: %d ;",height);
zmoutaou 0:6e330c197193 202 fprintf(fp,"weight: %d ;",weight);
zmoutaou 0:6e330c197193 203 fprintf(fp,"age: %d ;",age);
zmoutaou 0:6e330c197193 204 fprintf(fp,"gender: %s ;",gender);
zmoutaou 0:6e330c197193 205 fprintf(fp,"max_heart_rate: %d ;",max_heart_rate);
zmoutaou 0:6e330c197193 206 fprintf(fp,"\n********************************BLE-fin**************************************\n");
zmoutaou 0:6e330c197193 207 message_envoyee= false;
zmoutaou 0:6e330c197193 208 myledr =0 ;
zmoutaou 0:6e330c197193 209 interruption1= false;
zmoutaou 0:6e330c197193 210 oled.clearDisplay();
zmoutaou 0:6e330c197193 211 OLED_writeString(" welcome ", 32, 16);
zmoutaou 0:6e330c197193 212 oled.clearDisplay();
zmoutaou 0:6e330c197193 213 OLED_writeString(name, 32, 16);
zmoutaou 0:6e330c197193 214 } else {
zmoutaou 0:6e330c197193 215 ble.waitForEvent();
zmoutaou 0:6e330c197193 216 }
zmoutaou 0:6e330c197193 217 }
zmoutaou 0:6e330c197193 218
zmoutaou 0:6e330c197193 219 while ( (interruption2 == true) and (force_mesure ==false )) {
zmoutaou 0:6e330c197193 220 myledg =1;
zmoutaou 0:6e330c197193 221 if (size_data_i==0 ) {
zmoutaou 0:6e330c197193 222 fprintf(fp,"\n volts_measure pression_measure force_measure \n");
zmoutaou 0:6e330c197193 223 }
zmoutaou 0:6e330c197193 224 volts_measure[size_data_i] = tension_A502.read()*3.3 ;
zmoutaou 0:6e330c197193 225 pression_measure[size_data_i] = 9146.3*(pow_nc(volts_measure[size_data_i],3))-47648*(pow_nc(volts_measure[size_data_i],2))+103084*volts_measure[size_data_i]-47228;
zmoutaou 0:6e330c197193 226 force_measure[size_data_i] = pression_measure[size_data_i] *0.00193/9.81;
zmoutaou 0:6e330c197193 227 if( not (pression_measure[size_data_i]< 0 or force_measure[size_data_i]< 0) ) {
zmoutaou 0:6e330c197193 228 fprintf(fp,"%f %f %f \n",volts_measure[size_data_i],pression_measure[size_data_i],force_measure[size_data_i] );
zmoutaou 0:6e330c197193 229 pc.printf("%f %f %f \n",volts_measure[size_data_i],pression_measure[size_data_i],force_measure[size_data_i] );
zmoutaou 0:6e330c197193 230 oled.clearDisplay();
zmoutaou 0:6e330c197193 231 oled.setTextSize(2);
zmoutaou 0:6e330c197193 232 char res[20];
zmoutaou 0:6e330c197193 233 ftoa(force_measure[size_data_i], res, 3);
zmoutaou 0:6e330c197193 234 OLED_writeString(res, 1, 1);
zmoutaou 0:6e330c197193 235 size_data_i =size_data_i+1;
zmoutaou 0:6e330c197193 236 wait(0.5);
zmoutaou 0:6e330c197193 237 if (size_data_i ==size_data-1 ) {
zmoutaou 0:6e330c197193 238 interruption2 = false;
zmoutaou 0:6e330c197193 239 force_mesure = true ;
zmoutaou 0:6e330c197193 240 size_data_i =0;
zmoutaou 0:6e330c197193 241 myledg =0;
zmoutaou 0:6e330c197193 242 }
zmoutaou 0:6e330c197193 243
zmoutaou 0:6e330c197193 244 }
zmoutaou 0:6e330c197193 245
zmoutaou 0:6e330c197193 246 }
zmoutaou 0:6e330c197193 247 if ( force_mesure ==true ) {
zmoutaou 0:6e330c197193 248 oled.clearDisplay();
zmoutaou 0:6e330c197193 249 while (1) {
zmoutaou 0:6e330c197193 250 dt.start();
zmoutaou 0:6e330c197193 251 if (erreur_imu == true) {
zmoutaou 0:6e330c197193 252 int l = 0;
zmoutaou 0:6e330c197193 253 fprintf(fp,"\n********************************Gait **************************************\n");
zmoutaou 0:6e330c197193 254 fprintf(fp,"ax, ay, az, gx, gy,gz,mx, my,mz\n");
zmoutaou 0:6e330c197193 255 while (l<5000) {
zmoutaou 0:6e330c197193 256
zmoutaou 0:6e330c197193 257 if (readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_STATUS_REG) & 0x01) {
zmoutaou 0:6e330c197193 258 //Accelerometer new data available. // check if new accel data is ready
zmoutaou 0:6e330c197193 259 readAccelData(accelCount); // Read the x/y/z adc values // Now we'll calculate the accleration value into actual g's
zmoutaou 0:6e330c197193 260 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
zmoutaou 0:6e330c197193 261 ay = (float)accelCount[1]*aRes - accelBias[1];
zmoutaou 0:6e330c197193 262 az = (float)accelCount[2]*aRes - accelBias[2];
zmoutaou 0:6e330c197193 263 }
zmoutaou 0:6e330c197193 264 if (readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_STATUS_REG) & 0x02) { // check if new gyro data is ready
zmoutaou 0:6e330c197193 265 readGyroData(gyroCount); // Read the x/y/z adc values // Calculate the gyro value into actual degrees per second
zmoutaou 0:6e330c197193 266 gx = (float)gyroCount[0]*gRes - gyroBias1[0]; // get actual gyro value, this depends on scale being set
zmoutaou 0:6e330c197193 267 gy = (float)gyroCount[1]*gRes - gyroBias1[1];
zmoutaou 0:6e330c197193 268 gz = (float)gyroCount[2]*gRes - gyroBias1[2];
zmoutaou 0:6e330c197193 269 }
zmoutaou 0:6e330c197193 270
zmoutaou 0:6e330c197193 271 if (readByte(LSM9DS1M_ADDRESS, LSM9DS1M_STATUS_REG_M) & 0x08) { // check if new mag data is ready
zmoutaou 0:6e330c197193 272 readMagData(magCount); // Read the x/y/z adc values
zmoutaou 0:6e330c197193 273 mx = (float)magCount[0]*mRes; // - magBias[0]; // get actual magnetometer value, this depends on scale being set
zmoutaou 0:6e330c197193 274 my = (float)magCount[1]*mRes; // - magBias[1];
zmoutaou 0:6e330c197193 275 mz = (float)magCount[2]*mRes; // - magBias[2];}
zmoutaou 0:6e330c197193 276 }
zmoutaou 0:6e330c197193 277 l= l+1;
zmoutaou 0:6e330c197193 278 fprintf(fp,"\n %f %f %f ;%f %f %f ;%f %f %f ", ax, ay, az, gx, gy,gz,mx, my,mz);
zmoutaou 0:6e330c197193 279 wait_us(1);
zmoutaou 0:6e330c197193 280 }
zmoutaou 0:6e330c197193 281 } else
zmoutaou 0:6e330c197193 282 pc.printf("Erreur LSM9DS1");
zmoutaou 0:6e330c197193 283 }
zmoutaou 0:6e330c197193 284 fprintf(fp,"\n********************************GPS **************************************\n");
zmoutaou 0:6e330c197193 285 c = myGPS.read(); //queries the GPS
zmoutaou 0:6e330c197193 286 myledb = 0;
zmoutaou 0:6e330c197193 287 if (c) { }
zmoutaou 0:6e330c197193 288 if ( myGPS.newNMEAreceived() ) {
zmoutaou 0:6e330c197193 289 if ( !myGPS.parse(myGPS.lastNMEA())) {
zmoutaou 0:6e330c197193 290 continue;
zmoutaou 0:6e330c197193 291 }
zmoutaou 0:6e330c197193 292 }
zmoutaou 0:6e330c197193 293 if (refresh_Timer.read_ms() >= refresh_Time) {
zmoutaou 0:6e330c197193 294 refresh_Timer.reset();
zmoutaou 0:6e330c197193 295
zmoutaou 0:6e330c197193 296 if (myGPS.fix) {
zmoutaou 0:6e330c197193 297 myledb = 0.5;
zmoutaou 0:6e330c197193 298 fprintf(fp,"Time: %d:%d:%d.%u\n", myGPS.hour+1, myGPS.minute, myGPS.seconds, myGPS.milliseconds);
zmoutaou 0:6e330c197193 299 Now = millis()*0.001 ;
zmoutaou 0:6e330c197193 300 t = Now - t;
zmoutaou 0:6e330c197193 301 fprintf(fp,"Date: %d/%d/20%d\n", myGPS.day, myGPS.month, myGPS.year);
zmoutaou 0:6e330c197193 302 fprintf(fp,"Fix:. %d\n", (int) myGPS.fix);
zmoutaou 0:6e330c197193 303 fprintf(fp,"Quality: %d\n\n", (int) myGPS.fixquality);
zmoutaou 0:6e330c197193 304
zmoutaou 0:6e330c197193 305 pc.printf("Time: %d:%d:%d.%u\n", myGPS.hour+1, myGPS.minute, myGPS.seconds, myGPS.milliseconds);
zmoutaou 0:6e330c197193 306 pc.printf("Date: %d/%d/20%d\n", myGPS.day, myGPS.month, myGPS.year);
zmoutaou 0:6e330c197193 307 pc.printf("Fix: %d\n", (int) myGPS.fix);
zmoutaou 0:6e330c197193 308 pc.printf("Quality: %d\n\n", (int) myGPS.fixquality);
zmoutaou 0:6e330c197193 309 lat_dd = myGPS.coordToDegDec(myGPS.latitude);
zmoutaou 0:6e330c197193 310 lon_dd = myGPS.coordToDegDec(myGPS.longitude);
zmoutaou 0:6e330c197193 311 if (i_gps == 0) {
zmoutaou 0:6e330c197193 312 lat_1 = lat_dd;
zmoutaou 0:6e330c197193 313 lon_1 = lon_dd;
zmoutaou 0:6e330c197193 314 lat_2 = lat_1;
zmoutaou 0:6e330c197193 315 lon_2 = lon_1;
zmoutaou 0:6e330c197193 316 t_m = dt.read_ms() ;
zmoutaou 0:6e330c197193 317 t_m = dt.read();
zmoutaou 0:6e330c197193 318 } else {
zmoutaou 0:6e330c197193 319 lat_1 = lat_2;
zmoutaou 0:6e330c197193 320 lon_1 = lon_2;
zmoutaou 0:6e330c197193 321 lat_2 = lat_dd;
zmoutaou 0:6e330c197193 322 lon_2 = lon_dd;
zmoutaou 0:6e330c197193 323 t_apres = t_m ;
zmoutaou 0:6e330c197193 324 t_m = dt.read();
zmoutaou 0:6e330c197193 325 t_d = t_apres - t_m ;
zmoutaou 0:6e330c197193 326 ::distance = myGPS.getDistance(lat_1, lon_1, lat_2, lon_2);
zmoutaou 0:6e330c197193 327 ::sum_distance += ::distance;
zmoutaou 0:6e330c197193 328 ::steps = myGPS.getSteps(::sum_distance, height);
zmoutaou 0:6e330c197193 329 ::avg_speed = myGPS.getAvgSpeed(::sum_distance, t);
zmoutaou 0:6e330c197193 330 char res[20];
zmoutaou 0:6e330c197193 331 ftoa(steps, res, 1);
zmoutaou 0:6e330c197193 332 OLED_writeString(res, 1, 1);
zmoutaou 0:6e330c197193 333 }
zmoutaou 0:6e330c197193 334 fprintf(fp,"Satellites : %d\n", myGPS.satellites);
zmoutaou 0:6e330c197193 335 fprintf(fp,"Location (deg) : %5.5f%c, %5.5f%c\n", lat_dd, myGPS.lat, lon_dd, myGPS.lon);
zmoutaou 0:6e330c197193 336 fprintf(fp,"Altitude (m) : %5.2f\n", myGPS.altitude);
zmoutaou 0:6e330c197193 337 fprintf(fp,"Distance (m) : %5.2f\n", ::sum_distance);
zmoutaou 0:6e330c197193 338 fprintf(fp,"Steps taken : %5.0f\n", ::steps);
zmoutaou 0:6e330c197193 339 fprintf(fp,"Average speed (km/h) : %5.2f\n\n\n", ::avg_speed);
zmoutaou 0:6e330c197193 340 pc.printf("Satellites : %d\n", myGPS.satellites);
zmoutaou 0:6e330c197193 341 pc.printf("Location (deg) : %5.5f%c, %5.5f%c\n", lat_dd, myGPS.lat, lon_dd, myGPS.lon);
zmoutaou 0:6e330c197193 342 pc.printf("Altitude (m) : %5.2f\n", myGPS.altitude);
zmoutaou 0:6e330c197193 343 pc.printf("Distance (m) : %5.2f\n", ::sum_distance);
zmoutaou 0:6e330c197193 344 pc.printf("Steps taken : %5.0f\n", ::steps);
zmoutaou 0:6e330c197193 345 pc.printf("Average speed (km/h) : %5.5f\n\n\n", ::avg_speed);
zmoutaou 0:6e330c197193 346 myledb = 0;
zmoutaou 0:6e330c197193 347 i_gps++;
zmoutaou 0:6e330c197193 348 }
zmoutaou 0:6e330c197193 349 }
zmoutaou 0:6e330c197193 350 } // Closing the file
zmoutaou 0:6e330c197193 351 }
zmoutaou 0:6e330c197193 352 }
zmoutaou 0:6e330c197193 353
zmoutaou 0:6e330c197193 354
zmoutaou 0:6e330c197193 355
zmoutaou 0:6e330c197193 356
zmoutaou 0:6e330c197193 357
zmoutaou 0:6e330c197193 358 void fct_name_text(uint8_t newState)
zmoutaou 0:6e330c197193 359 {
zmoutaou 0:6e330c197193 360 name[i] = (char) newState;
zmoutaou 0:6e330c197193 361 i= i+1 ;
zmoutaou 0:6e330c197193 362 pc.printf("name %s ",name);
zmoutaou 0:6e330c197193 363 }
zmoutaou 0:6e330c197193 364 void fct_height_int(uint8_t newState)
zmoutaou 0:6e330c197193 365 {
zmoutaou 0:6e330c197193 366 height = (int) newState;
zmoutaou 0:6e330c197193 367 pc.printf("height %d ",height);
zmoutaou 0:6e330c197193 368 }
zmoutaou 0:6e330c197193 369 void fct_weight_int(uint8_t newState)
zmoutaou 0:6e330c197193 370 {
zmoutaou 0:6e330c197193 371 weight = (int) newState;
zmoutaou 0:6e330c197193 372 pc.printf("weight %d ",weight);
zmoutaou 0:6e330c197193 373 }
zmoutaou 0:6e330c197193 374 void fct_age_int(uint8_t newState)
zmoutaou 0:6e330c197193 375 {
zmoutaou 0:6e330c197193 376 age = (int) newState;
zmoutaou 0:6e330c197193 377 pc.printf("age %d ",age);
zmoutaou 0:6e330c197193 378 }
zmoutaou 0:6e330c197193 379 void fct_gender_text(uint8_t newState)
zmoutaou 0:6e330c197193 380 {
zmoutaou 0:6e330c197193 381 gender[0] = (char) newState;
zmoutaou 0:6e330c197193 382 pc.printf("gender %s ",gender);
zmoutaou 0:6e330c197193 383 }
zmoutaou 0:6e330c197193 384 void fct_max_heart_rate_int(uint8_t newState)
zmoutaou 0:6e330c197193 385 {
zmoutaou 0:6e330c197193 386 max_heart_rate = (int) newState;
zmoutaou 0:6e330c197193 387 pc.printf("max_heart_rate %d ",max_heart_rate);
zmoutaou 0:6e330c197193 388 }
zmoutaou 0:6e330c197193 389 void fct_fini(uint8_t newState)
zmoutaou 0:6e330c197193 390 {
zmoutaou 0:6e330c197193 391 message_envoyee= true;
zmoutaou 0:6e330c197193 392 pc.printf("message_envoyee ");
zmoutaou 0:6e330c197193 393 }
zmoutaou 0:6e330c197193 394
zmoutaou 0:6e330c197193 395 void fct_interruption_ble()
zmoutaou 0:6e330c197193 396 {
zmoutaou 0:6e330c197193 397 interruption1= true ;
zmoutaou 0:6e330c197193 398 height = 0;
zmoutaou 0:6e330c197193 399 i=0;
zmoutaou 0:6e330c197193 400 for (int j=1; j<sizeof(name); j++) {
zmoutaou 0:6e330c197193 401 name[j] = ' ';
zmoutaou 0:6e330c197193 402 }
zmoutaou 0:6e330c197193 403 weight=0;
zmoutaou 0:6e330c197193 404 age =0;
zmoutaou 0:6e330c197193 405 char gender[1] = {'_'} ;
zmoutaou 0:6e330c197193 406 max_heart_rate=0;
zmoutaou 0:6e330c197193 407 message_envoyee= false;
zmoutaou 0:6e330c197193 408 }
zmoutaou 0:6e330c197193 409
zmoutaou 0:6e330c197193 410 void fct_interruption_force()
zmoutaou 0:6e330c197193 411 {
zmoutaou 0:6e330c197193 412 interruption2 = true ;
zmoutaou 0:6e330c197193 413 force_mesure = false;
zmoutaou 0:6e330c197193 414 }
zmoutaou 0:6e330c197193 415
zmoutaou 0:6e330c197193 416
zmoutaou 0:6e330c197193 417 void OLED_writeString(string str, int x, int y)
zmoutaou 0:6e330c197193 418 {
zmoutaou 0:6e330c197193 419 oled.setTextCursor(x, y);
zmoutaou 0:6e330c197193 420 oled.fillRect(x, y, 128, 8, 0);
zmoutaou 0:6e330c197193 421
zmoutaou 0:6e330c197193 422 for (int k = 0; k < str.length(); k++) {
zmoutaou 0:6e330c197193 423 oled.writeChar(str[k]);
zmoutaou 0:6e330c197193 424 }
zmoutaou 0:6e330c197193 425
zmoutaou 0:6e330c197193 426 oled.display();
zmoutaou 0:6e330c197193 427 }
zmoutaou 0:6e330c197193 428
zmoutaou 0:6e330c197193 429 void ftoa(float n, char* res, int afterpoint) //https://www.geeksforgeeks.org/convert-floating-point-number-string/
zmoutaou 0:6e330c197193 430 {
zmoutaou 0:6e330c197193 431
zmoutaou 0:6e330c197193 432 int ipart = (int)n; // Extract integer part
zmoutaou 0:6e330c197193 433 float fpart = n - (float)ipart; // Extract floating part
zmoutaou 0:6e330c197193 434 int k = intToStr(ipart, res, 0); // convert integer part to string
zmoutaou 0:6e330c197193 435 if (afterpoint != 0) { // check for display option after point
zmoutaou 0:6e330c197193 436 res[k] = '.'; // a
zmoutaou 0:6e330c197193 437 fpart = fpart * pow_nc(10, afterpoint);
zmoutaou 0:6e330c197193 438 intToStr((int)fpart, res + k + 1, afterpoint);
zmoutaou 0:6e330c197193 439 }
zmoutaou 0:6e330c197193 440 }
zmoutaou 0:6e330c197193 441
zmoutaou 0:6e330c197193 442 int intToStr(int x, char str[], int d)
zmoutaou 0:6e330c197193 443 {
zmoutaou 0:6e330c197193 444 int k = 0;
zmoutaou 0:6e330c197193 445 while (x) {
zmoutaou 0:6e330c197193 446 str[k++] = (x % 10) + '0';
zmoutaou 0:6e330c197193 447 x = x / 10;
zmoutaou 0:6e330c197193 448 }
zmoutaou 0:6e330c197193 449
zmoutaou 0:6e330c197193 450 // If number of digits required is more, then
zmoutaou 0:6e330c197193 451 // add 0s at the beginning
zmoutaou 0:6e330c197193 452 while (k < d)
zmoutaou 0:6e330c197193 453 str[k++] = '0';
zmoutaou 0:6e330c197193 454
zmoutaou 0:6e330c197193 455 reverse(str, k);
zmoutaou 0:6e330c197193 456 str[k] = '\0';
zmoutaou 0:6e330c197193 457 return k;
zmoutaou 0:6e330c197193 458 }
zmoutaou 0:6e330c197193 459
zmoutaou 0:6e330c197193 460
zmoutaou 0:6e330c197193 461 void reverse(char* str, int len)
zmoutaou 0:6e330c197193 462 {
zmoutaou 0:6e330c197193 463 int k = 0, j = len - 1, temp;
zmoutaou 0:6e330c197193 464 while (k < j) {
zmoutaou 0:6e330c197193 465 temp = str[k];
zmoutaou 0:6e330c197193 466 str[k] = str[j];
zmoutaou 0:6e330c197193 467 str[j] = temp;
zmoutaou 0:6e330c197193 468 k++;
zmoutaou 0:6e330c197193 469 j--;
zmoutaou 0:6e330c197193 470 }
zmoutaou 0:6e330c197193 471 }
zmoutaou 0:6e330c197193 472
zmoutaou 0:6e330c197193 473
zmoutaou 0:6e330c197193 474 float pow_nc(float n, int l)
zmoutaou 0:6e330c197193 475 {
zmoutaou 0:6e330c197193 476 for (int k=1; k<l; k++) {
zmoutaou 0:6e330c197193 477 n= n*n ;
zmoutaou 0:6e330c197193 478 }
zmoutaou 0:6e330c197193 479 return(n);
zmoutaou 0:6e330c197193 480 }
zmoutaou 0:6e330c197193 481
zmoutaou 0:6e330c197193 482
zmoutaou 0:6e330c197193 483 // Mostra no display uma mensagem de receptividade
zmoutaou 0:6e330c197193 484 void OLED_drawSmile()
zmoutaou 0:6e330c197193 485 {
zmoutaou 0:6e330c197193 486 oled.clearDisplay();
zmoutaou 0:6e330c197193 487 oled.drawCircle(12, 20, 9, 1);
zmoutaou 0:6e330c197193 488 oled.fillRect(0, 5, 22, 15, 0);
zmoutaou 0:6e330c197193 489 oled.fillTriangle(2, 15, 10, 15, 6, 7, 1);
zmoutaou 0:6e330c197193 490 oled.fillTriangle(3, 15, 9, 15, 6, 8, 0);
zmoutaou 0:6e330c197193 491 oled.fillTriangle(14, 15, 22, 15, 18, 7, 1);
zmoutaou 0:6e330c197193 492 oled.fillTriangle(15, 15, 21, 15, 18, 8, 0);
zmoutaou 0:6e330c197193 493 oled.clearDisplay();
zmoutaou 0:6e330c197193 494
zmoutaou 0:6e330c197193 495 }
zmoutaou 0:6e330c197193 496 void selftestLSM9DS1()
zmoutaou 0:6e330c197193 497 {
zmoutaou 0:6e330c197193 498 float accel_noST[3] = {0., 0., 0.}, accel_ST[3] = {0., 0., 0.};
zmoutaou 0:6e330c197193 499 float gyro_noST[3] = {0., 0., 0.}, gyro_ST[3] = {0., 0., 0.};
zmoutaou 0:6e330c197193 500 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG10, 0x00); // disable self test
zmoutaou 0:6e330c197193 501 accelgyrocalLSM9DS1(gyro_noST, accel_noST);
zmoutaou 0:6e330c197193 502 ////pc.printf("disable self test ");
zmoutaou 0:6e330c197193 503 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG10, 0x05); // enable gyro/accel self test
zmoutaou 0:6e330c197193 504 accelgyrocalLSM9DS1(gyro_ST, accel_ST);
zmoutaou 0:6e330c197193 505 ////pc.printf("enable gyro/accel self test");
zmoutaou 0:6e330c197193 506
zmoutaou 0:6e330c197193 507 float gyrodx = (gyro_ST[0] - gyro_noST[0]);
zmoutaou 0:6e330c197193 508 float gyrody = (gyro_ST[1] - gyro_noST[1]);
zmoutaou 0:6e330c197193 509 float gyrodz = (gyro_ST[2] - gyro_noST[2]);
zmoutaou 0:6e330c197193 510
zmoutaou 0:6e330c197193 511 ////pc.printf("Gyro self-test results: ");
zmoutaou 0:6e330c197193 512 ////pc.printf("x-axis = %f , dps" ,gyrodx); //////////pc.printf(" should be between 20 and 250 dps \n");
zmoutaou 0:6e330c197193 513 ////pc.printf("y-axis = %f ",gyrody); //////////pc.printf(" dps"); //////pc.printf(" should be between 20 and 250 dps \n");
zmoutaou 0:6e330c197193 514 ////pc.printf("z-axis = %f ",gyrodz); //////////pc.printf(" dps"); //////pc.printf(" should be between 20 and 250 dps \n");
zmoutaou 0:6e330c197193 515
zmoutaou 0:6e330c197193 516 float accdx = 1000.*(accel_ST[0] - accel_noST[0]);
zmoutaou 0:6e330c197193 517 float accdy = 1000.*(accel_ST[1] - accel_noST[1]);
zmoutaou 0:6e330c197193 518 float accdz = 1000.*(accel_ST[2] - accel_noST[2]);
zmoutaou 0:6e330c197193 519 //pc.printf( "%f,%f,%f,%f,%f,%f\n", gyrodx,gyrody,gyrodz,accdx,accdy,accdz);
zmoutaou 0:6e330c197193 520 ////pc.printf("Accelerometer self-test results: ");
zmoutaou 0:6e330c197193 521 ////pc.printf("x-axis = %f" , accdx); //////////pc.printf(" mg"); //////pc.printf(" should be between 60 and 1700 mg \n");
zmoutaou 0:6e330c197193 522 ////pc.printf("y-axis = %f" , accdy); //////////pc.printf(" mg"); //////pc.printf(" should be between 60 and 1700 mg \n");
zmoutaou 0:6e330c197193 523 ////pc.printf("z-axis = %f", accdz); ////////pc.printf(" mg"); //////pc.printf(" should be between 60 and 1700 mg \n");
zmoutaou 0:6e330c197193 524
zmoutaou 0:6e330c197193 525 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG10, 0x00); // disable self test
zmoutaou 0:6e330c197193 526
zmoutaou 0:6e330c197193 527 wait(0.2);
zmoutaou 0:6e330c197193 528
zmoutaou 0:6e330c197193 529 }
zmoutaou 0:6e330c197193 530
zmoutaou 0:6e330c197193 531
zmoutaou 0:6e330c197193 532
zmoutaou 0:6e330c197193 533 void initLSM9DS1()
zmoutaou 0:6e330c197193 534 {
zmoutaou 0:6e330c197193 535 // enable the 3-axes of the gyroscope
zmoutaou 0:6e330c197193 536 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG4, 0x38);
zmoutaou 0:6e330c197193 537 ////pc.printf( "enable the 3-axes of the gyroscope\n");
zmoutaou 0:6e330c197193 538 // configure the gyroscope
zmoutaou 0:6e330c197193 539 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG1_G, Godr << 5 | Gscale << 3 | Gbw);
zmoutaou 0:6e330c197193 540 ////pc.printf("// configure the gyroscope\n");
zmoutaou 0:6e330c197193 541 wait(0.2);
zmoutaou 0:6e330c197193 542 // enable the three axes of the accelerometer
zmoutaou 0:6e330c197193 543 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG5_XL, 0x38);
zmoutaou 0:6e330c197193 544 ////pc.printf("// enable the three axes of the accelerometer\n ") ;
zmoutaou 0:6e330c197193 545 // configure the accelerometer-specify bandwidth selection with Abw
zmoutaou 0:6e330c197193 546 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG6_XL, Aodr << 5 | Ascale << 3 | 0x04 |Abw);
zmoutaou 0:6e330c197193 547 ////pc.printf("configure the accelerometer-specify bandwidth selection with Abw\n");
zmoutaou 0:6e330c197193 548 wait(0.2);
zmoutaou 0:6e330c197193 549 // enable block data update, allow auto-increment during multiple byte read
zmoutaou 0:6e330c197193 550 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG8, 0x44);
zmoutaou 0:6e330c197193 551 ////pc.printf("enable block data update, allow auto-increment during multiple byte read\n");
zmoutaou 0:6e330c197193 552 // configure the magnetometer-enable temperature compensation of mag data
zmoutaou 0:6e330c197193 553 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG1_M, 0x80 | Mmode << 5 | Modr << 2); // select x,y-axis mode
zmoutaou 0:6e330c197193 554 ////pc.printf("configure the magnetometer-enable temperature compensation of mag data\n");
zmoutaou 0:6e330c197193 555 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG2_M, Mscale << 5 ); // select mag full scale
zmoutaou 0:6e330c197193 556 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG3_M, 0x00 ); // continuous conversion mode
zmoutaou 0:6e330c197193 557 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG4_M, Mmode << 2 ); // select z-axis mode
zmoutaou 0:6e330c197193 558 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG5_M, 0x40 ); // select block update mode
zmoutaou 0:6e330c197193 559 }
zmoutaou 0:6e330c197193 560 void getMres()
zmoutaou 0:6e330c197193 561
zmoutaou 0:6e330c197193 562 {
zmoutaou 0:6e330c197193 563 switch (Mscale) {
zmoutaou 0:6e330c197193 564 // Possible magnetometer scales (and their register bit settings) are:
zmoutaou 0:6e330c197193 565 // 4 Gauss (00), 8 Gauss (01), 12 Gauss (10) and 16 Gauss (11)
zmoutaou 0:6e330c197193 566 case MFS_4G:
zmoutaou 0:6e330c197193 567 mRes = 4.0/32768.0;
zmoutaou 0:6e330c197193 568 break;
zmoutaou 0:6e330c197193 569 case MFS_8G:
zmoutaou 0:6e330c197193 570 mRes = 8.0/32768.0;
zmoutaou 0:6e330c197193 571 break;
zmoutaou 0:6e330c197193 572 case MFS_12G:
zmoutaou 0:6e330c197193 573 mRes = 12.0/32768.0;
zmoutaou 0:6e330c197193 574 break;
zmoutaou 0:6e330c197193 575 case MFS_16G:
zmoutaou 0:6e330c197193 576 mRes = 16.0/32768.0;
zmoutaou 0:6e330c197193 577 break;
zmoutaou 0:6e330c197193 578 }
zmoutaou 0:6e330c197193 579 }
zmoutaou 0:6e330c197193 580
zmoutaou 0:6e330c197193 581
zmoutaou 0:6e330c197193 582 void getGres()
zmoutaou 0:6e330c197193 583
zmoutaou 0:6e330c197193 584 {
zmoutaou 0:6e330c197193 585 switch (Gscale) {
zmoutaou 0:6e330c197193 586 // Possible gyro scales (and their register bit settings) are:
zmoutaou 0:6e330c197193 587 // 245 DPS (00), 500 DPS (01), and 2000 DPS (11).
zmoutaou 0:6e330c197193 588 case GFS_245DPS:
zmoutaou 0:6e330c197193 589 gRes = 245.0/32768.0;//2^15
zmoutaou 0:6e330c197193 590 break;
zmoutaou 0:6e330c197193 591 case GFS_500DPS:
zmoutaou 0:6e330c197193 592 gRes = 500.0/32768.0;
zmoutaou 0:6e330c197193 593 break;
zmoutaou 0:6e330c197193 594 case GFS_2000DPS:
zmoutaou 0:6e330c197193 595 gRes = 2000.0/32768.0;
zmoutaou 0:6e330c197193 596 break;
zmoutaou 0:6e330c197193 597 }
zmoutaou 0:6e330c197193 598 }
zmoutaou 0:6e330c197193 599
zmoutaou 0:6e330c197193 600
zmoutaou 0:6e330c197193 601 void getAres()
zmoutaou 0:6e330c197193 602
zmoutaou 0:6e330c197193 603 {
zmoutaou 0:6e330c197193 604 switch (Ascale) {
zmoutaou 0:6e330c197193 605 // Possible accelerometer scales (and their register bit settings) are:
zmoutaou 0:6e330c197193 606 // 2 Gs (00), 16 Gs (01), 4 Gs (10), and 8 Gs (11).
zmoutaou 0:6e330c197193 607 case AFS_2G:
zmoutaou 0:6e330c197193 608 aRes = 2.0/32768.0;
zmoutaou 0:6e330c197193 609 break;
zmoutaou 0:6e330c197193 610 case AFS_16G:
zmoutaou 0:6e330c197193 611 aRes = 16.0/32768.0;
zmoutaou 0:6e330c197193 612 break;
zmoutaou 0:6e330c197193 613 case AFS_4G:
zmoutaou 0:6e330c197193 614 aRes = 4.0/32768.0;
zmoutaou 0:6e330c197193 615 break;
zmoutaou 0:6e330c197193 616 case AFS_8G:
zmoutaou 0:6e330c197193 617 aRes = 8.0/32768.0;
zmoutaou 0:6e330c197193 618 break;
zmoutaou 0:6e330c197193 619 }
zmoutaou 0:6e330c197193 620 }
zmoutaou 0:6e330c197193 621
zmoutaou 0:6e330c197193 622 void writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
zmoutaou 0:6e330c197193 623
zmoutaou 0:6e330c197193 624 {
zmoutaou 0:6e330c197193 625 char temp_data[2] = {subAddress, data};
zmoutaou 0:6e330c197193 626 i2c_m.write(address, temp_data, 2);
zmoutaou 0:6e330c197193 627 }
zmoutaou 0:6e330c197193 628 uint8_t readByte(uint8_t address, uint8_t subAddress)
zmoutaou 0:6e330c197193 629
zmoutaou 0:6e330c197193 630 {
zmoutaou 0:6e330c197193 631 char data;
zmoutaou 0:6e330c197193 632 char temp[1] = {subAddress};
zmoutaou 0:6e330c197193 633 i2c_m.write(address, temp, 1);
zmoutaou 0:6e330c197193 634 temp[1] = 0x00;
zmoutaou 0:6e330c197193 635 i2c_m.write(address, temp, 1);
zmoutaou 0:6e330c197193 636 int a = i2c_m.read(address, &data, 1);
zmoutaou 0:6e330c197193 637 return data;
zmoutaou 0:6e330c197193 638
zmoutaou 0:6e330c197193 639 }
zmoutaou 0:6e330c197193 640 void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
zmoutaou 0:6e330c197193 641
zmoutaou 0:6e330c197193 642 {
zmoutaou 0:6e330c197193 643
zmoutaou 0:6e330c197193 644 int i;
zmoutaou 0:6e330c197193 645 char temp_dest[count];
zmoutaou 0:6e330c197193 646 char temp[1] = {subAddress};
zmoutaou 0:6e330c197193 647 i2c_m.write(address, temp, 1);
zmoutaou 0:6e330c197193 648 i2c_m.read(address, temp_dest, count);
zmoutaou 0:6e330c197193 649 for (i=0; i < count; i++) {
zmoutaou 0:6e330c197193 650 dest[i] = temp_dest[i];
zmoutaou 0:6e330c197193 651 }
zmoutaou 0:6e330c197193 652 }
zmoutaou 0:6e330c197193 653
zmoutaou 0:6e330c197193 654
zmoutaou 0:6e330c197193 655 void accelgyrocalLSM9DS1(float * dest1, float * dest2)
zmoutaou 0:6e330c197193 656
zmoutaou 0:6e330c197193 657 {
zmoutaou 0:6e330c197193 658 uint8_t data[6] = {0};
zmoutaou 0:6e330c197193 659 int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
zmoutaou 0:6e330c197193 660 uint16_t samples, ii;
zmoutaou 0:6e330c197193 661 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG4, 0x38); // enable the 3-axes of the gyroscope
zmoutaou 0:6e330c197193 662 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG1_G, Godr << 5 | Gscale << 3 | Gbw); // configure the gyroscope
zmoutaou 0:6e330c197193 663 wait(0.2);
zmoutaou 0:6e330c197193 664 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG5_XL, 0x38); // enable the three axes of the accelerometer
zmoutaou 0:6e330c197193 665 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG6_XL, Aodr << 5 | Ascale << 3 | 0x04 |Abw); // configure the accelerometer-specify bandwidth selection with Abw
zmoutaou 0:6e330c197193 666 wait(0.2);
zmoutaou 0:6e330c197193 667 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG8, 0x44);// enable block data update, allow auto-increment during multiple byte read
zmoutaou 0:6e330c197193 668 uint8_t c = readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9);// First get gyro bias
zmoutaou 0:6e330c197193 669 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9, (c & 0xFD)| 0x02); // Enable gyro FIFO
zmoutaou 0:6e330c197193 670 wait(0.050); // Wait for change to take effect
zmoutaou 0:6e330c197193 671 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_CTRL, 0x20 | 0x1F); // Enable gyro FIFO stream mode and set watermark at 32 samples
zmoutaou 0:6e330c197193 672 wait(1); // delay 1000 milliseconds to collect FIFO samples
zmoutaou 0:6e330c197193 673 samples = (readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_SRC) & 0x2F); // Read number of stored samples
zmoutaou 0:6e330c197193 674 for(ii = 0; ii < samples ; ii++) {
zmoutaou 0:6e330c197193 675 // Read the gyro data stored in the FIFO
zmoutaou 0:6e330c197193 676 int16_t gyro_temp[3] = {0, 0, 0};
zmoutaou 0:6e330c197193 677 readBytes(LSM9DS1XG_ADDRESS, LSM9DS1XG_OUT_X_L_G, 6, &data[0]);
zmoutaou 0:6e330c197193 678 gyro_temp[0] = (int16_t) (((int16_t)data[1] << 8) | data[0]); // Form signed 16-bit integer for each sample in FIFO
zmoutaou 0:6e330c197193 679 gyro_temp[1] = (int16_t) (((int16_t)data[3] << 8) | data[2]);
zmoutaou 0:6e330c197193 680 gyro_temp[2] = (int16_t) (((int16_t)data[5] << 8) | data[4]);
zmoutaou 0:6e330c197193 681
zmoutaou 0:6e330c197193 682 gyro_bias[0] += (int32_t) gyro_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
zmoutaou 0:6e330c197193 683 gyro_bias[1] += (int32_t) gyro_temp[1];
zmoutaou 0:6e330c197193 684 gyro_bias[2] += (int32_t) gyro_temp[2];
zmoutaou 0:6e330c197193 685 }
zmoutaou 0:6e330c197193 686
zmoutaou 0:6e330c197193 687 gyro_bias[0] /= samples; // average the data
zmoutaou 0:6e330c197193 688 gyro_bias[1] /= samples;
zmoutaou 0:6e330c197193 689 gyro_bias[2] /= samples;
zmoutaou 0:6e330c197193 690
zmoutaou 0:6e330c197193 691 dest1[0] = (float)gyro_bias[0]*gRes; // Properly scale the data to get deg/s
zmoutaou 0:6e330c197193 692 dest1[1] = (float)gyro_bias[1]*gRes;
zmoutaou 0:6e330c197193 693 dest1[2] = (float)gyro_bias[2]*gRes;
zmoutaou 0:6e330c197193 694
zmoutaou 0:6e330c197193 695 c = readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9);
zmoutaou 0:6e330c197193 696 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9, c & ~0x02); //Disable gyro FIFO
zmoutaou 0:6e330c197193 697 wait(0.050);
zmoutaou 0:6e330c197193 698 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_CTRL, 0x00); // Enable gyro bypass mode
zmoutaou 0:6e330c197193 699
zmoutaou 0:6e330c197193 700 // now get the accelerometer bias
zmoutaou 0:6e330c197193 701 c = readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9);
zmoutaou 0:6e330c197193 702 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9, (c & 0xFD)| 0x02); // Enable accel FIFO
zmoutaou 0:6e330c197193 703 wait(0.050); // Wait for change to take effect
zmoutaou 0:6e330c197193 704 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_CTRL, 0x20 | 0x1F); // Enable accel FIFO stream mode and set watermark at 32 samples
zmoutaou 0:6e330c197193 705 wait(1); // delay 1000 milliseconds to collect FIFO samples
zmoutaou 0:6e330c197193 706
zmoutaou 0:6e330c197193 707 samples = (readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_SRC) & 0x2F); // Read number of stored samples
zmoutaou 0:6e330c197193 708
zmoutaou 0:6e330c197193 709 for(ii = 0; ii < samples ; ii++) { // Read the accel data stored in the FIFO
zmoutaou 0:6e330c197193 710 int16_t accel_temp[3] = {0, 0, 0};
zmoutaou 0:6e330c197193 711 readBytes(LSM9DS1XG_ADDRESS, LSM9DS1XG_OUT_X_L_XL, 6, &data[0]);
zmoutaou 0:6e330c197193 712 accel_temp[0] = (int16_t) (((int16_t)data[1] << 8) | data[0]); // Form signed 16-bit integer for each sample in FIFO
zmoutaou 0:6e330c197193 713 accel_temp[1] = (int16_t) (((int16_t)data[3] << 8) | data[2]);
zmoutaou 0:6e330c197193 714 accel_temp[2] = (int16_t) (((int16_t)data[5] << 8) | data[4]);
zmoutaou 0:6e330c197193 715
zmoutaou 0:6e330c197193 716 accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
zmoutaou 0:6e330c197193 717 accel_bias[1] += (int32_t) accel_temp[1];
zmoutaou 0:6e330c197193 718 accel_bias[2] += (int32_t) accel_temp[2];
zmoutaou 0:6e330c197193 719 }
zmoutaou 0:6e330c197193 720
zmoutaou 0:6e330c197193 721 accel_bias[0] /= samples; // average the data
zmoutaou 0:6e330c197193 722 accel_bias[1] /= samples;
zmoutaou 0:6e330c197193 723 accel_bias[2] /= samples;
zmoutaou 0:6e330c197193 724
zmoutaou 0:6e330c197193 725 if(accel_bias[2] > 0L) {
zmoutaou 0:6e330c197193 726 accel_bias[2] -= (int32_t) (1.0/aRes); // Remove gravity from the z-axis accelerometer bias calculation
zmoutaou 0:6e330c197193 727 } else {
zmoutaou 0:6e330c197193 728 accel_bias[2] += (int32_t) (1.0/aRes);
zmoutaou 0:6e330c197193 729 }
zmoutaou 0:6e330c197193 730
zmoutaou 0:6e330c197193 731 dest2[0] = (float)accel_bias[0]*aRes; // Properly scale the data to get g
zmoutaou 0:6e330c197193 732 dest2[1] = (float)accel_bias[1]*aRes;
zmoutaou 0:6e330c197193 733 dest2[2] = (float)accel_bias[2]*aRes;
zmoutaou 0:6e330c197193 734
zmoutaou 0:6e330c197193 735 c = readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9);
zmoutaou 0:6e330c197193 736 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9, c & ~0x02); //Disable accel FIFO
zmoutaou 0:6e330c197193 737 wait(0.050);
zmoutaou 0:6e330c197193 738 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_CTRL, 0x00); // Enable accel bypass mode*/
zmoutaou 0:6e330c197193 739 }
zmoutaou 0:6e330c197193 740 void magcalLSM9DS1(float * dest1)
zmoutaou 0:6e330c197193 741
zmoutaou 0:6e330c197193 742 {
zmoutaou 0:6e330c197193 743 uint8_t data[6]; // data array to hold mag x, y, z, data
zmoutaou 0:6e330c197193 744 uint16_t ii = 0, sample_count = 0;
zmoutaou 0:6e330c197193 745 int32_t mag_bias[3] = {0, 0, 0};
zmoutaou 0:6e330c197193 746 int16_t mag_max[3] = {0, 0, 0}, mag_min[3] = {0, 0, 0};
zmoutaou 0:6e330c197193 747
zmoutaou 0:6e330c197193 748 // configure the magnetometer-enable temperature compensation of mag data
zmoutaou 0:6e330c197193 749 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG1_M, 0x80 | Mmode << 5 | Modr << 2); // select x,y-axis mode
zmoutaou 0:6e330c197193 750 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG2_M, Mscale << 5 ); // select mag full scale
zmoutaou 0:6e330c197193 751 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG3_M, 0x00 ); // continuous conversion mode
zmoutaou 0:6e330c197193 752 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG4_M, Mmode << 2 ); // select z-axis mode
zmoutaou 0:6e330c197193 753 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG5_M, 0x40 ); // select block update mode
zmoutaou 0:6e330c197193 754 ////pc.printf("Mag Calibration: Wave device in a figure eight until done!\n");
zmoutaou 0:6e330c197193 755 wait(0.004);
zmoutaou 0:6e330c197193 756
zmoutaou 0:6e330c197193 757 sample_count = 128;
zmoutaou 0:6e330c197193 758 for(ii = 0; ii < sample_count; ii++) {
zmoutaou 0:6e330c197193 759 int16_t mag_temp[3] = {0, 0, 0};
zmoutaou 0:6e330c197193 760 readBytes(LSM9DS1M_ADDRESS, LSM9DS1M_OUT_X_L_M, 6, &data[0]); // Read the six raw data registers into data array
zmoutaou 0:6e330c197193 761 mag_temp[0] = (int16_t) (((int16_t)data[1] << 8) | data[0]) ; // Form signed 16-bit integer for each sample in FIFO
zmoutaou 0:6e330c197193 762 mag_temp[1] = (int16_t) (((int16_t)data[3] << 8) | data[2]) ;
zmoutaou 0:6e330c197193 763 mag_temp[2] = (int16_t) (((int16_t)data[5] << 8) | data[4]) ;
zmoutaou 0:6e330c197193 764 for (int jj = 0; jj < 3; jj++) {
zmoutaou 0:6e330c197193 765 if(mag_temp[jj] > mag_max[jj]) mag_max[jj] = mag_temp[jj];
zmoutaou 0:6e330c197193 766 if(mag_temp[jj] < mag_min[jj]) mag_min[jj] = mag_temp[jj];
zmoutaou 0:6e330c197193 767 }
zmoutaou 0:6e330c197193 768 wait(0.105); // at 10 Hz ODR, new mag data is available every 100 ms
zmoutaou 0:6e330c197193 769 }
zmoutaou 0:6e330c197193 770
zmoutaou 0:6e330c197193 771 // ////pc.printf("mag x min/max:"); //pc.printfmag_max[0]); //pc.printfmag_min[0]);
zmoutaou 0:6e330c197193 772 // ////pc.printf("mag y min/max:"); //pc.printfmag_max[1]); //pc.printfmag_min[1]);
zmoutaou 0:6e330c197193 773 // ////pc.printf("mag z min/max:"); //pc.printfmag_max[2]); //pc.printfmag_min[2]);
zmoutaou 0:6e330c197193 774
zmoutaou 0:6e330c197193 775 mag_bias[0] = (mag_max[0] + mag_min[0])/2; // get average x mag bias in counts
zmoutaou 0:6e330c197193 776 mag_bias[1] = (mag_max[1] + mag_min[1])/2; // get average y mag bias in counts
zmoutaou 0:6e330c197193 777 mag_bias[2] = (mag_max[2] + mag_min[2])/2; // get average z mag bias in counts
zmoutaou 0:6e330c197193 778
zmoutaou 0:6e330c197193 779 dest1[0] = (float) mag_bias[0]*mRes; // save mag biases in G for main program
zmoutaou 0:6e330c197193 780 dest1[1] = (float) mag_bias[1]*mRes;
zmoutaou 0:6e330c197193 781 dest1[2] = (float) mag_bias[2]*mRes;
zmoutaou 0:6e330c197193 782
zmoutaou 0:6e330c197193 783 //write biases to accelerometermagnetometer offset registers as counts);
zmoutaou 0:6e330c197193 784 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_OFFSET_X_REG_L_M, (int16_t) mag_bias[0] & 0xFF);
zmoutaou 0:6e330c197193 785 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_OFFSET_X_REG_H_M, ((int16_t)mag_bias[0] >> 8) & 0xFF);
zmoutaou 0:6e330c197193 786 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_OFFSET_Y_REG_L_M, (int16_t) mag_bias[1] & 0xFF);
zmoutaou 0:6e330c197193 787 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_OFFSET_Y_REG_H_M, ((int16_t)mag_bias[1] >> 8) & 0xFF);
zmoutaou 0:6e330c197193 788 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_OFFSET_Z_REG_L_M, (int16_t) mag_bias[2] & 0xFF);
zmoutaou 0:6e330c197193 789 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_OFFSET_Z_REG_H_M, ((int16_t)mag_bias[2] >> 8) & 0xFF);
zmoutaou 0:6e330c197193 790
zmoutaou 0:6e330c197193 791 ////pc.printf("Mag Calibration done!\n");
zmoutaou 0:6e330c197193 792 }
zmoutaou 0:6e330c197193 793
zmoutaou 0:6e330c197193 794 void readAccelData(int16_t * destination)
zmoutaou 0:6e330c197193 795
zmoutaou 0:6e330c197193 796 {
zmoutaou 0:6e330c197193 797 uint8_t rawData[6]; // x/y/z accel register data stored here
zmoutaou 0:6e330c197193 798 readBytes(LSM9DS1XG_ADDRESS, LSM9DS1XG_OUT_X_L_XL, 6, &rawData[0]); // Read the six raw data registers into data array
zmoutaou 0:6e330c197193 799 destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value
zmoutaou 0:6e330c197193 800 destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ;
zmoutaou 0:6e330c197193 801 destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ;
zmoutaou 0:6e330c197193 802 }
zmoutaou 0:6e330c197193 803
zmoutaou 0:6e330c197193 804 void readGyroData(int16_t * destination)
zmoutaou 0:6e330c197193 805
zmoutaou 0:6e330c197193 806 {
zmoutaou 0:6e330c197193 807 uint8_t rawData[6]; // x/y/z gyro register data stored here
zmoutaou 0:6e330c197193 808 readBytes(LSM9DS1XG_ADDRESS, LSM9DS1XG_OUT_X_L_G, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
zmoutaou 0:6e330c197193 809 destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value
zmoutaou 0:6e330c197193 810 destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ;
zmoutaou 0:6e330c197193 811 destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ;
zmoutaou 0:6e330c197193 812 }
zmoutaou 0:6e330c197193 813
zmoutaou 0:6e330c197193 814 void readMagData(int16_t * destination)
zmoutaou 0:6e330c197193 815 {
zmoutaou 0:6e330c197193 816 uint8_t rawData[6]; // x/y/z gyro register data stored here
zmoutaou 0:6e330c197193 817 readBytes(LSM9DS1M_ADDRESS, LSM9DS1M_OUT_X_L_M, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
zmoutaou 0:6e330c197193 818 destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value
zmoutaou 0:6e330c197193 819 destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ; // Data stored as little Endian
zmoutaou 0:6e330c197193 820 destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ;
zmoutaou 0:6e330c197193 821 }
zmoutaou 0:6e330c197193 822
zmoutaou 0:6e330c197193 823
zmoutaou 0:6e330c197193 824
zmoutaou 0:6e330c197193 825
zmoutaou 0:6e330c197193 826
zmoutaou 0:6e330c197193 827
zmoutaou 0:6e330c197193 828
zmoutaou 0:6e330c197193 829
zmoutaou 0:6e330c197193 830
zmoutaou 0:6e330c197193 831