![](/media/cache/group/logo_mPY59fz.jpg.50x50_q85.jpg)
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?
User | Revision | Line number | New 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 |