ENSMM / Mbed 2 deprecated lsm9ds1_1

lsm9ds1 not organized

Dependencies:   mbed SDFileSystem USBDevice

main.cpp@0:70465b25c44f, 2020-01-27 (annotated)

Committer:
zmoutaou
Date:
Mon Jan 27 10:50:13 2020 +0000
Revision:
0:70465b25c44f
lsm9ds1 not organized

Who changed what in which revision?

UserRevisionLine numberNew contents of line
zmoutaou 0:70465b25c44f 1 #include "mbed.h"
zmoutaou 0:70465b25c44f 2 #include "USBSerial.h"
zmoutaou 0:70465b25c44f 3 #include "lsm9ds1.h"
zmoutaou 0:70465b25c44f 4 #include "SDFileSystem.h"
zmoutaou 0:70465b25c44f 5
zmoutaou 0:70465b25c44f 6
zmoutaou 0:70465b25c44f 7
zmoutaou 0:70465b25c44f 8
zmoutaou 0:70465b25c44f 9 USBSerial pc(0x1f00, 0x2012, 0x0001, false);
zmoutaou 0:70465b25c44f 10 Timer t;
zmoutaou 0:70465b25c44f 11 int lastPrint = 0;
zmoutaou 0:70465b25c44f 12 int printInterval = 1;
zmoutaou 0:70465b25c44f 13
zmoutaou 0:70465b25c44f 14
zmoutaou 0:70465b25c44f 15
zmoutaou 0:70465b25c44f 16 void magcalLSM9DS1(float * dest1) ;
zmoutaou 0:70465b25c44f 17 void accelgyrocalLSM9DS1(float * dest1, float * dest2);
zmoutaou 0:70465b25c44f 18 void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest);
zmoutaou 0:70465b25c44f 19 uint8_t readByte(uint8_t address, uint8_t subAddress);
zmoutaou 0:70465b25c44f 20 void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest);
zmoutaou 0:70465b25c44f 21 void writeByte(uint8_t address, uint8_t subAddress, uint8_t data);
zmoutaou 0:70465b25c44f 22 void getAres();
zmoutaou 0:70465b25c44f 23 void getGres();
zmoutaou 0:70465b25c44f 24 void getMres();
zmoutaou 0:70465b25c44f 25 void readAccelData(int16_t * destination);
zmoutaou 0:70465b25c44f 26 void readGyroData(int16_t * destination);
zmoutaou 0:70465b25c44f 27 void readMagData(int16_t * destination);
zmoutaou 0:70465b25c44f 28 void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz);
zmoutaou 0:70465b25c44f 29 int16_t readTempData();
zmoutaou 0:70465b25c44f 30 void initLSM9DS1();
zmoutaou 0:70465b25c44f 31 void selftestLSM9DS1();
zmoutaou 0:70465b25c44f 32 void LSM9DS1_start( int &erreur );
zmoutaou 0:70465b25c44f 33 void LSM9DS1_VARS(float &ax, float &ay, float &az, float &mx, float &my, float &mz,float &anglex,float &angley,float &anglez, float &gx, float &gy, float &gz, bool &erreur, float &yaw, float &pitch,float &roll );
zmoutaou 0:70465b25c44f 34
zmoutaou 0:70465b25c44f 35 I2C i2c_m(PB_9, PB_8);
zmoutaou 0:70465b25c44f 36 unsigned char nCRC; // calculated check sum to ensure PROM integrity
zmoutaou 0:70465b25c44f 37 double dT, OFFSET, SENS, T2, OFFSET2, SENS2; // First order and second order corrections for raw S5637 temperature and pressure data
zmoutaou 0:70465b25c44f 38 int16_t accelCount[3], gyroCount[3], magCount[3]; // Stores the 16-bit signed accelerometer, gyro, and mag sensor output
zmoutaou 0:70465b25c44f 39 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:70465b25c44f 40 int16_t tempCount; // temperature raw count output
zmoutaou 0:70465b25c44f 41 float temperature; // Stores the LSM9DS1gyro internal chip temperature in degrees Celsius
zmoutaou 0:70465b25c44f 42 double Temperature, Pressure; // stores MS5611 pressures sensor pressure and temperature
zmoutaou 0:70465b25c44f 43 // global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System)
zmoutaou 0:70465b25c44f 44 float GyroMeasError = PI * (40.0f / 180.0f); // gyroscope measurement error in rads/s (start at 40 deg/s)
zmoutaou 0:70465b25c44f 45 float GyroMeasDrift = PI * (0.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
zmoutaou 0:70465b25c44f 46 // There is a tradeoff in the beta parameter between accuracy and response speed.
zmoutaou 0:70465b25c44f 47 // 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:70465b25c44f 48 // However, with this value, the LSM9SD0 response time is about 10 seconds to a stable initial quaternion.
zmoutaou 0:70465b25c44f 49 // Subsequent changes also require a longish lag time to a stable output, not fast enough for a quadcopter or robot car!
zmoutaou 0:70465b25c44f 50 // By increasing beta (GyroMeasError) by about a factor of fifteen, the response time constant is reduced to ~2 sec
zmoutaou 0:70465b25c44f 51 // I haven't noticed any reduction in solution accuracy. This is essentially the I coefficient in a PID control sense;
zmoutaou 0:70465b25c44f 52 // the bigger the feedback coefficient, the faster the solution converges, usually at the expense of accuracy.
zmoutaou 0:70465b25c44f 53 // In any case, this is the free parameter in the Madgwick filtering and fusion scheme.
zmoutaou 0:70465b25c44f 54 float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta
zmoutaou 0:70465b25c44f 55 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:70465b25c44f 56
zmoutaou 0:70465b25c44f 57
zmoutaou 0:70465b25c44f 58 uint32_t delt_t = 0, count = 0, sumCount = 0; // used to control display output rate
zmoutaou 0:70465b25c44f 59 float pitch, yaw, roll;
zmoutaou 0:70465b25c44f 60 float deltat = 0.0f, sum = 0.0f; // integration interval for both filter schemes
zmoutaou 0:70465b25c44f 61 uint32_t lastUpdate = 0, firstUpdate = 0; // used to calculate integration interval
zmoutaou 0:70465b25c44f 62 uint32_t Now = 0; // used to calculate integration interval
zmoutaou 0:70465b25c44f 63
zmoutaou 0:70465b25c44f 64 float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values
zmoutaou 0:70465b25c44f 65 float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion
zmoutaou 0:70465b25c44f 66 float eInt[3] = {0.0f, 0.0f, 0.0f}; // vector to hold integral error for Mahony meth
zmoutaou 0:70465b25c44f 67
zmoutaou 0:70465b25c44f 68 // Specify sensor full scale
zmoutaou 0:70465b25c44f 69 uint8_t OSR = ADC_4096; // set pressure amd temperature oversample rate
zmoutaou 0:70465b25c44f 70 uint8_t Gscale = GFS_245DPS; // gyro full scale
zmoutaou 0:70465b25c44f 71 uint8_t Godr = GODR_238Hz; // gyro data sample rate
zmoutaou 0:70465b25c44f 72 uint8_t Gbw = GBW_med; // gyro data bandwidth
zmoutaou 0:70465b25c44f 73 uint8_t Ascale = AFS_2G; // accel full scale
zmoutaou 0:70465b25c44f 74 uint8_t Aodr = AODR_238Hz; // accel data sample rate
zmoutaou 0:70465b25c44f 75 uint8_t Abw = ABW_50Hz; // accel data bandwidth
zmoutaou 0:70465b25c44f 76 uint8_t Mscale = MFS_4G; // mag full scale
zmoutaou 0:70465b25c44f 77 uint8_t Modr = MODR_10Hz; // mag data sample rate
zmoutaou 0:70465b25c44f 78 uint8_t Mmode = MMode_HighPerformance; // magnetometer operation mode
zmoutaou 0:70465b25c44f 79 float aRes, gRes, mRes; // scale resolutions per LSB for the sensors
zmoutaou 0:70465b25c44f 80
zmoutaou 0:70465b25c44f 81 void getMres()
zmoutaou 0:70465b25c44f 82 {
zmoutaou 0:70465b25c44f 83 switch (Mscale) {
zmoutaou 0:70465b25c44f 84 // Possible magnetometer scales (and their register bit settings) are:
zmoutaou 0:70465b25c44f 85 // 4 Gauss (00), 8 Gauss (01), 12 Gauss (10) and 16 Gauss (11)
zmoutaou 0:70465b25c44f 86 case MFS_4G:
zmoutaou 0:70465b25c44f 87 mRes = 4.0/32768.0;
zmoutaou 0:70465b25c44f 88 break;
zmoutaou 0:70465b25c44f 89 case MFS_8G:
zmoutaou 0:70465b25c44f 90 mRes = 8.0/32768.0;
zmoutaou 0:70465b25c44f 91 break;
zmoutaou 0:70465b25c44f 92 case MFS_12G:
zmoutaou 0:70465b25c44f 93 mRes = 12.0/32768.0;
zmoutaou 0:70465b25c44f 94 break;
zmoutaou 0:70465b25c44f 95 case MFS_16G:
zmoutaou 0:70465b25c44f 96 mRes = 16.0/32768.0;
zmoutaou 0:70465b25c44f 97 break;
zmoutaou 0:70465b25c44f 98 }
zmoutaou 0:70465b25c44f 99 }
zmoutaou 0:70465b25c44f 100
zmoutaou 0:70465b25c44f 101
zmoutaou 0:70465b25c44f 102
zmoutaou 0:70465b25c44f 103 void getGres()
zmoutaou 0:70465b25c44f 104 {
zmoutaou 0:70465b25c44f 105 switch (Gscale) {
zmoutaou 0:70465b25c44f 106 // Possible gyro scales (and their register bit settings) are:
zmoutaou 0:70465b25c44f 107 // 245 DPS (00), 500 DPS (01), and 2000 DPS (11).
zmoutaou 0:70465b25c44f 108 case GFS_245DPS:
zmoutaou 0:70465b25c44f 109 gRes = 245.0/32768.0;//2^15
zmoutaou 0:70465b25c44f 110 break;
zmoutaou 0:70465b25c44f 111 case GFS_500DPS:
zmoutaou 0:70465b25c44f 112 gRes = 500.0/32768.0;
zmoutaou 0:70465b25c44f 113 break;
zmoutaou 0:70465b25c44f 114 case GFS_2000DPS:
zmoutaou 0:70465b25c44f 115 gRes = 2000.0/32768.0;
zmoutaou 0:70465b25c44f 116 break;
zmoutaou 0:70465b25c44f 117 }
zmoutaou 0:70465b25c44f 118 }
zmoutaou 0:70465b25c44f 119
zmoutaou 0:70465b25c44f 120
zmoutaou 0:70465b25c44f 121
zmoutaou 0:70465b25c44f 122 void getAres()
zmoutaou 0:70465b25c44f 123 {
zmoutaou 0:70465b25c44f 124 switch (Ascale) {
zmoutaou 0:70465b25c44f 125 // Possible accelerometer scales (and their register bit settings) are:
zmoutaou 0:70465b25c44f 126 // 2 Gs (00), 16 Gs (01), 4 Gs (10), and 8 Gs (11).
zmoutaou 0:70465b25c44f 127 case AFS_2G:
zmoutaou 0:70465b25c44f 128 aRes = 2.0/32768.0;
zmoutaou 0:70465b25c44f 129 break;
zmoutaou 0:70465b25c44f 130 case AFS_16G:
zmoutaou 0:70465b25c44f 131 aRes = 16.0/32768.0;
zmoutaou 0:70465b25c44f 132 break;
zmoutaou 0:70465b25c44f 133 case AFS_4G:
zmoutaou 0:70465b25c44f 134 aRes = 4.0/32768.0;
zmoutaou 0:70465b25c44f 135 break;
zmoutaou 0:70465b25c44f 136 case AFS_8G:
zmoutaou 0:70465b25c44f 137 aRes = 8.0/32768.0;
zmoutaou 0:70465b25c44f 138 break;
zmoutaou 0:70465b25c44f 139 }
zmoutaou 0:70465b25c44f 140 }
zmoutaou 0:70465b25c44f 141
zmoutaou 0:70465b25c44f 142
zmoutaou 0:70465b25c44f 143 void writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
zmoutaou 0:70465b25c44f 144 {
zmoutaou 0:70465b25c44f 145 char temp_data[2] = {subAddress, data};
zmoutaou 0:70465b25c44f 146 i2c_m.write(address, temp_data, 2);
zmoutaou 0:70465b25c44f 147 }
zmoutaou 0:70465b25c44f 148
zmoutaou 0:70465b25c44f 149 uint8_t readByte(uint8_t address, uint8_t subAddress)
zmoutaou 0:70465b25c44f 150 {
zmoutaou 0:70465b25c44f 151 char data;
zmoutaou 0:70465b25c44f 152 char temp[1] = {subAddress};
zmoutaou 0:70465b25c44f 153 i2c_m.write(address, temp, 1);
zmoutaou 0:70465b25c44f 154 temp[1] = 0x00;
zmoutaou 0:70465b25c44f 155 i2c_m.write(address, temp, 1);
zmoutaou 0:70465b25c44f 156 int a = i2c_m.read(address, &data, 1);
zmoutaou 0:70465b25c44f 157 return data;
zmoutaou 0:70465b25c44f 158
zmoutaou 0:70465b25c44f 159 }
zmoutaou 0:70465b25c44f 160
zmoutaou 0:70465b25c44f 161 void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
zmoutaou 0:70465b25c44f 162 {
zmoutaou 0:70465b25c44f 163
zmoutaou 0:70465b25c44f 164 int i;
zmoutaou 0:70465b25c44f 165 char temp_dest[count];
zmoutaou 0:70465b25c44f 166 char temp[1] = {subAddress};
zmoutaou 0:70465b25c44f 167 i2c_m.write(address, temp, 1);
zmoutaou 0:70465b25c44f 168 i2c_m.read(address, temp_dest, count);
zmoutaou 0:70465b25c44f 169 for (i=0; i < count; i++) {
zmoutaou 0:70465b25c44f 170 dest[i] = temp_dest[i];
zmoutaou 0:70465b25c44f 171 }
zmoutaou 0:70465b25c44f 172 }
zmoutaou 0:70465b25c44f 173
zmoutaou 0:70465b25c44f 174
zmoutaou 0:70465b25c44f 175 void accelgyrocalLSM9DS1(float * dest1, float * dest2)
zmoutaou 0:70465b25c44f 176 {
zmoutaou 0:70465b25c44f 177 uint8_t data[6] = {0};
zmoutaou 0:70465b25c44f 178 int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
zmoutaou 0:70465b25c44f 179 uint16_t samples, ii;
zmoutaou 0:70465b25c44f 180 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG4, 0x38); // enable the 3-axes of the gyroscope
zmoutaou 0:70465b25c44f 181 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG1_G, Godr << 5 | Gscale << 3 | Gbw); // configure the gyroscope
zmoutaou 0:70465b25c44f 182 wait(0.2);
zmoutaou 0:70465b25c44f 183 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG5_XL, 0x38); // enable the three axes of the accelerometer
zmoutaou 0:70465b25c44f 184 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG6_XL, Aodr << 5 | Ascale << 3 | 0x04 |Abw); // configure the accelerometer-specify bandwidth selection with Abw
zmoutaou 0:70465b25c44f 185 wait(0.2);
zmoutaou 0:70465b25c44f 186 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG8, 0x44);// enable block data update, allow auto-increment during multiple byte read
zmoutaou 0:70465b25c44f 187 uint8_t c = readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9);// First get gyro bias
zmoutaou 0:70465b25c44f 188 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9, (c & 0xFD)| 0x02); // Enable gyro FIFO
zmoutaou 0:70465b25c44f 189 wait(0.050); // Wait for change to take effect
zmoutaou 0:70465b25c44f 190 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_CTRL, 0x20 | 0x1F); // Enable gyro FIFO stream mode and set watermark at 32 samples
zmoutaou 0:70465b25c44f 191 wait(1); // delay 1000 milliseconds to collect FIFO samples
zmoutaou 0:70465b25c44f 192 samples = (readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_SRC) & 0x2F); // Read number of stored samples
zmoutaou 0:70465b25c44f 193 for(ii = 0; ii < samples ; ii++) {
zmoutaou 0:70465b25c44f 194 // Read the gyro data stored in the FIFO
zmoutaou 0:70465b25c44f 195 int16_t gyro_temp[3] = {0, 0, 0};
zmoutaou 0:70465b25c44f 196 readBytes(LSM9DS1XG_ADDRESS, LSM9DS1XG_OUT_X_L_G, 6, &data[0]);
zmoutaou 0:70465b25c44f 197 gyro_temp[0] = (int16_t) (((int16_t)data[1] << 8) | data[0]); // Form signed 16-bit integer for each sample in FIFO
zmoutaou 0:70465b25c44f 198 gyro_temp[1] = (int16_t) (((int16_t)data[3] << 8) | data[2]);
zmoutaou 0:70465b25c44f 199 gyro_temp[2] = (int16_t) (((int16_t)data[5] << 8) | data[4]);
zmoutaou 0:70465b25c44f 200
zmoutaou 0:70465b25c44f 201 gyro_bias[0] += (int32_t) gyro_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
zmoutaou 0:70465b25c44f 202 gyro_bias[1] += (int32_t) gyro_temp[1];
zmoutaou 0:70465b25c44f 203 gyro_bias[2] += (int32_t) gyro_temp[2];
zmoutaou 0:70465b25c44f 204 }
zmoutaou 0:70465b25c44f 205
zmoutaou 0:70465b25c44f 206 gyro_bias[0] /= samples; // average the data
zmoutaou 0:70465b25c44f 207 gyro_bias[1] /= samples;
zmoutaou 0:70465b25c44f 208 gyro_bias[2] /= samples;
zmoutaou 0:70465b25c44f 209
zmoutaou 0:70465b25c44f 210 dest1[0] = (float)gyro_bias[0]*gRes; // Properly scale the data to get deg/s
zmoutaou 0:70465b25c44f 211 dest1[1] = (float)gyro_bias[1]*gRes;
zmoutaou 0:70465b25c44f 212 dest1[2] = (float)gyro_bias[2]*gRes;
zmoutaou 0:70465b25c44f 213
zmoutaou 0:70465b25c44f 214 c = readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9);
zmoutaou 0:70465b25c44f 215 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9, c & ~0x02); //Disable gyro FIFO
zmoutaou 0:70465b25c44f 216 wait(0.050);
zmoutaou 0:70465b25c44f 217 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_CTRL, 0x00); // Enable gyro bypass mode
zmoutaou 0:70465b25c44f 218
zmoutaou 0:70465b25c44f 219 // now get the accelerometer bias
zmoutaou 0:70465b25c44f 220 c = readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9);
zmoutaou 0:70465b25c44f 221 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9, (c & 0xFD)| 0x02); // Enable accel FIFO
zmoutaou 0:70465b25c44f 222 wait(0.050); // Wait for change to take effect
zmoutaou 0:70465b25c44f 223 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_CTRL, 0x20 | 0x1F); // Enable accel FIFO stream mode and set watermark at 32 samples
zmoutaou 0:70465b25c44f 224 wait(1); // delay 1000 milliseconds to collect FIFO samples
zmoutaou 0:70465b25c44f 225
zmoutaou 0:70465b25c44f 226 samples = (readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_SRC) & 0x2F); // Read number of stored samples
zmoutaou 0:70465b25c44f 227
zmoutaou 0:70465b25c44f 228 for(ii = 0; ii < samples ; ii++) { // Read the accel data stored in the FIFO
zmoutaou 0:70465b25c44f 229 int16_t accel_temp[3] = {0, 0, 0};
zmoutaou 0:70465b25c44f 230 readBytes(LSM9DS1XG_ADDRESS, LSM9DS1XG_OUT_X_L_XL, 6, &data[0]);
zmoutaou 0:70465b25c44f 231 accel_temp[0] = (int16_t) (((int16_t)data[1] << 8) | data[0]); // Form signed 16-bit integer for each sample in FIFO
zmoutaou 0:70465b25c44f 232 accel_temp[1] = (int16_t) (((int16_t)data[3] << 8) | data[2]);
zmoutaou 0:70465b25c44f 233 accel_temp[2] = (int16_t) (((int16_t)data[5] << 8) | data[4]);
zmoutaou 0:70465b25c44f 234
zmoutaou 0:70465b25c44f 235 accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
zmoutaou 0:70465b25c44f 236 accel_bias[1] += (int32_t) accel_temp[1];
zmoutaou 0:70465b25c44f 237 accel_bias[2] += (int32_t) accel_temp[2];
zmoutaou 0:70465b25c44f 238 }
zmoutaou 0:70465b25c44f 239
zmoutaou 0:70465b25c44f 240 accel_bias[0] /= samples; // average the data
zmoutaou 0:70465b25c44f 241 accel_bias[1] /= samples;
zmoutaou 0:70465b25c44f 242 accel_bias[2] /= samples;
zmoutaou 0:70465b25c44f 243
zmoutaou 0:70465b25c44f 244 if(accel_bias[2] > 0L) {
zmoutaou 0:70465b25c44f 245 accel_bias[2] -= (int32_t) (1.0/aRes); // Remove gravity from the z-axis accelerometer bias calculation
zmoutaou 0:70465b25c44f 246 } else {
zmoutaou 0:70465b25c44f 247 accel_bias[2] += (int32_t) (1.0/aRes);
zmoutaou 0:70465b25c44f 248 }
zmoutaou 0:70465b25c44f 249
zmoutaou 0:70465b25c44f 250 dest2[0] = (float)accel_bias[0]*aRes; // Properly scale the data to get g
zmoutaou 0:70465b25c44f 251 dest2[1] = (float)accel_bias[1]*aRes;
zmoutaou 0:70465b25c44f 252 dest2[2] = (float)accel_bias[2]*aRes;
zmoutaou 0:70465b25c44f 253
zmoutaou 0:70465b25c44f 254 c = readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9);
zmoutaou 0:70465b25c44f 255 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9, c & ~0x02); //Disable accel FIFO
zmoutaou 0:70465b25c44f 256 wait(0.050);
zmoutaou 0:70465b25c44f 257 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_CTRL, 0x00); // Enable accel bypass mode*/
zmoutaou 0:70465b25c44f 258 }
zmoutaou 0:70465b25c44f 259
zmoutaou 0:70465b25c44f 260 void magcalLSM9DS1(float * dest1)
zmoutaou 0:70465b25c44f 261 {
zmoutaou 0:70465b25c44f 262 uint8_t data[6]; // data array to hold mag x, y, z, data
zmoutaou 0:70465b25c44f 263 uint16_t ii = 0, sample_count = 0;
zmoutaou 0:70465b25c44f 264 int32_t mag_bias[3] = {0, 0, 0};
zmoutaou 0:70465b25c44f 265 int16_t mag_max[3] = {0, 0, 0}, mag_min[3] = {0, 0, 0};
zmoutaou 0:70465b25c44f 266
zmoutaou 0:70465b25c44f 267 // configure the magnetometer-enable temperature compensation of mag data
zmoutaou 0:70465b25c44f 268 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG1_M, 0x80 | Mmode << 5 | Modr << 2); // select x,y-axis mode
zmoutaou 0:70465b25c44f 269 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG2_M, Mscale << 5 ); // select mag full scale
zmoutaou 0:70465b25c44f 270 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG3_M, 0x00 ); // continuous conversion mode
zmoutaou 0:70465b25c44f 271 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG4_M, Mmode << 2 ); // select z-axis mode
zmoutaou 0:70465b25c44f 272 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG5_M, 0x40 ); // select block update mode
zmoutaou 0:70465b25c44f 273 ////pc.printf("Mag Calibration: Wave device in a figure eight until done!\n");
zmoutaou 0:70465b25c44f 274 wait(0.004);
zmoutaou 0:70465b25c44f 275
zmoutaou 0:70465b25c44f 276 sample_count = 128;
zmoutaou 0:70465b25c44f 277 for(ii = 0; ii < sample_count; ii++) {
zmoutaou 0:70465b25c44f 278 int16_t mag_temp[3] = {0, 0, 0};
zmoutaou 0:70465b25c44f 279 readBytes(LSM9DS1M_ADDRESS, LSM9DS1M_OUT_X_L_M, 6, &data[0]); // Read the six raw data registers into data array
zmoutaou 0:70465b25c44f 280 mag_temp[0] = (int16_t) (((int16_t)data[1] << 8) | data[0]) ; // Form signed 16-bit integer for each sample in FIFO
zmoutaou 0:70465b25c44f 281 mag_temp[1] = (int16_t) (((int16_t)data[3] << 8) | data[2]) ;
zmoutaou 0:70465b25c44f 282 mag_temp[2] = (int16_t) (((int16_t)data[5] << 8) | data[4]) ;
zmoutaou 0:70465b25c44f 283 for (int jj = 0; jj < 3; jj++) {
zmoutaou 0:70465b25c44f 284 if(mag_temp[jj] > mag_max[jj]) mag_max[jj] = mag_temp[jj];
zmoutaou 0:70465b25c44f 285 if(mag_temp[jj] < mag_min[jj]) mag_min[jj] = mag_temp[jj];
zmoutaou 0:70465b25c44f 286 }
zmoutaou 0:70465b25c44f 287 wait(0.105); // at 10 Hz ODR, new mag data is available every 100 ms
zmoutaou 0:70465b25c44f 288 }
zmoutaou 0:70465b25c44f 289
zmoutaou 0:70465b25c44f 290 // ////pc.printf("mag x min/max:"); //pc.printfmag_max[0]); //pc.printfmag_min[0]);
zmoutaou 0:70465b25c44f 291 // ////pc.printf("mag y min/max:"); //pc.printfmag_max[1]); //pc.printfmag_min[1]);
zmoutaou 0:70465b25c44f 292 // ////pc.printf("mag z min/max:"); //pc.printfmag_max[2]); //pc.printfmag_min[2]);
zmoutaou 0:70465b25c44f 293
zmoutaou 0:70465b25c44f 294 mag_bias[0] = (mag_max[0] + mag_min[0])/2; // get average x mag bias in counts
zmoutaou 0:70465b25c44f 295 mag_bias[1] = (mag_max[1] + mag_min[1])/2; // get average y mag bias in counts
zmoutaou 0:70465b25c44f 296 mag_bias[2] = (mag_max[2] + mag_min[2])/2; // get average z mag bias in counts
zmoutaou 0:70465b25c44f 297
zmoutaou 0:70465b25c44f 298 dest1[0] = (float) mag_bias[0]*mRes; // save mag biases in G for main program
zmoutaou 0:70465b25c44f 299 dest1[1] = (float) mag_bias[1]*mRes;
zmoutaou 0:70465b25c44f 300 dest1[2] = (float) mag_bias[2]*mRes;
zmoutaou 0:70465b25c44f 301
zmoutaou 0:70465b25c44f 302 //write biases to accelerometermagnetometer offset registers as counts);
zmoutaou 0:70465b25c44f 303 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_OFFSET_X_REG_L_M, (int16_t) mag_bias[0] & 0xFF);
zmoutaou 0:70465b25c44f 304 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_OFFSET_X_REG_H_M, ((int16_t)mag_bias[0] >> 8) & 0xFF);
zmoutaou 0:70465b25c44f 305 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_OFFSET_Y_REG_L_M, (int16_t) mag_bias[1] & 0xFF);
zmoutaou 0:70465b25c44f 306 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_OFFSET_Y_REG_H_M, ((int16_t)mag_bias[1] >> 8) & 0xFF);
zmoutaou 0:70465b25c44f 307 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_OFFSET_Z_REG_L_M, (int16_t) mag_bias[2] & 0xFF);
zmoutaou 0:70465b25c44f 308 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_OFFSET_Z_REG_H_M, ((int16_t)mag_bias[2] >> 8) & 0xFF);
zmoutaou 0:70465b25c44f 309
zmoutaou 0:70465b25c44f 310 ////pc.printf("Mag Calibration done!\n");
zmoutaou 0:70465b25c44f 311 }
zmoutaou 0:70465b25c44f 312
zmoutaou 0:70465b25c44f 313
zmoutaou 0:70465b25c44f 314 void readAccelData(int16_t * destination)
zmoutaou 0:70465b25c44f 315 {
zmoutaou 0:70465b25c44f 316 uint8_t rawData[6]; // x/y/z accel register data stored here
zmoutaou 0:70465b25c44f 317 readBytes(LSM9DS1XG_ADDRESS, LSM9DS1XG_OUT_X_L_XL, 6, &rawData[0]); // Read the six raw data registers into data array
zmoutaou 0:70465b25c44f 318 destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value
zmoutaou 0:70465b25c44f 319 destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ;
zmoutaou 0:70465b25c44f 320 destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ;
zmoutaou 0:70465b25c44f 321 }
zmoutaou 0:70465b25c44f 322
zmoutaou 0:70465b25c44f 323
zmoutaou 0:70465b25c44f 324 void readGyroData(int16_t * destination)
zmoutaou 0:70465b25c44f 325 {
zmoutaou 0:70465b25c44f 326 uint8_t rawData[6]; // x/y/z gyro register data stored here
zmoutaou 0:70465b25c44f 327 readBytes(LSM9DS1XG_ADDRESS, LSM9DS1XG_OUT_X_L_G, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
zmoutaou 0:70465b25c44f 328 destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value
zmoutaou 0:70465b25c44f 329 destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ;
zmoutaou 0:70465b25c44f 330 destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ;
zmoutaou 0:70465b25c44f 331 }
zmoutaou 0:70465b25c44f 332
zmoutaou 0:70465b25c44f 333 void readMagData(int16_t * destination)
zmoutaou 0:70465b25c44f 334 {
zmoutaou 0:70465b25c44f 335 uint8_t rawData[6]; // x/y/z gyro register data stored here
zmoutaou 0:70465b25c44f 336 readBytes(LSM9DS1M_ADDRESS, LSM9DS1M_OUT_X_L_M, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
zmoutaou 0:70465b25c44f 337 destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value
zmoutaou 0:70465b25c44f 338 destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ; // Data stored as little Endian
zmoutaou 0:70465b25c44f 339 destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ;
zmoutaou 0:70465b25c44f 340 }
zmoutaou 0:70465b25c44f 341 void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
zmoutaou 0:70465b25c44f 342 {
zmoutaou 0:70465b25c44f 343 float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability
zmoutaou 0:70465b25c44f 344 float norm;
zmoutaou 0:70465b25c44f 345 float hx, hy, bx, bz;
zmoutaou 0:70465b25c44f 346 float vx, vy, vz, wx, wy, wz;
zmoutaou 0:70465b25c44f 347 float ex, ey, ez;
zmoutaou 0:70465b25c44f 348 float pa, pb, pc;
zmoutaou 0:70465b25c44f 349
zmoutaou 0:70465b25c44f 350 // Auxiliary variables to avoid repeated arithmetic
zmoutaou 0:70465b25c44f 351 float q1q1 = q1 * q1;
zmoutaou 0:70465b25c44f 352 float q1q2 = q1 * q2;
zmoutaou 0:70465b25c44f 353 float q1q3 = q1 * q3;
zmoutaou 0:70465b25c44f 354 float q1q4 = q1 * q4;
zmoutaou 0:70465b25c44f 355 float q2q2 = q2 * q2;
zmoutaou 0:70465b25c44f 356 float q2q3 = q2 * q3;
zmoutaou 0:70465b25c44f 357 float q2q4 = q2 * q4;
zmoutaou 0:70465b25c44f 358 float q3q3 = q3 * q3;
zmoutaou 0:70465b25c44f 359 float q3q4 = q3 * q4;
zmoutaou 0:70465b25c44f 360 float q4q4 = q4 * q4;
zmoutaou 0:70465b25c44f 361
zmoutaou 0:70465b25c44f 362 // Normalise accelerometer measurement
zmoutaou 0:70465b25c44f 363 norm = sqrtf(ax * ax + ay * ay + az * az);
zmoutaou 0:70465b25c44f 364 if (norm == 0.0f) return; // handle NaN
zmoutaou 0:70465b25c44f 365 norm = 1.0f / norm; // use reciprocal for division
zmoutaou 0:70465b25c44f 366 ax *= norm;
zmoutaou 0:70465b25c44f 367 ay *= norm;
zmoutaou 0:70465b25c44f 368 az *= norm;
zmoutaou 0:70465b25c44f 369
zmoutaou 0:70465b25c44f 370 // Normalise magnetometer measurement
zmoutaou 0:70465b25c44f 371 norm = sqrtf(mx * mx + my * my + mz * mz);
zmoutaou 0:70465b25c44f 372 if (norm == 0.0f) return; // handle NaN
zmoutaou 0:70465b25c44f 373 norm = 1.0f / norm; // use reciprocal for division
zmoutaou 0:70465b25c44f 374 mx *= norm;
zmoutaou 0:70465b25c44f 375 my *= norm;
zmoutaou 0:70465b25c44f 376 mz *= norm;
zmoutaou 0:70465b25c44f 377
zmoutaou 0:70465b25c44f 378 // Reference direction of Earth's magnetic field
zmoutaou 0:70465b25c44f 379 hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3);
zmoutaou 0:70465b25c44f 380 hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2);
zmoutaou 0:70465b25c44f 381 bx = sqrtf((hx * hx) + (hy * hy));
zmoutaou 0:70465b25c44f 382 bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3);
zmoutaou 0:70465b25c44f 383
zmoutaou 0:70465b25c44f 384 // Estimated direction of gravity and magnetic field
zmoutaou 0:70465b25c44f 385 vx = 2.0f * (q2q4 - q1q3);
zmoutaou 0:70465b25c44f 386 vy = 2.0f * (q1q2 + q3q4);
zmoutaou 0:70465b25c44f 387 vz = q1q1 - q2q2 - q3q3 + q4q4;
zmoutaou 0:70465b25c44f 388 wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3);
zmoutaou 0:70465b25c44f 389 wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4);
zmoutaou 0:70465b25c44f 390 wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3);
zmoutaou 0:70465b25c44f 391
zmoutaou 0:70465b25c44f 392 // Error is cross product between estimated direction and measured direction of gravity
zmoutaou 0:70465b25c44f 393 ex = (ay * vz - az * vy) + (my * wz - mz * wy);
zmoutaou 0:70465b25c44f 394 ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
zmoutaou 0:70465b25c44f 395 ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
zmoutaou 0:70465b25c44f 396 if (Ki > 0.0f) {
zmoutaou 0:70465b25c44f 397 eInt[0] += ex; // accumulate integral error
zmoutaou 0:70465b25c44f 398 eInt[1] += ey;
zmoutaou 0:70465b25c44f 399 eInt[2] += ez;
zmoutaou 0:70465b25c44f 400 } else {
zmoutaou 0:70465b25c44f 401 eInt[0] = 0.0f; // prevent integral wind up
zmoutaou 0:70465b25c44f 402 eInt[1] = 0.0f;
zmoutaou 0:70465b25c44f 403 eInt[2] = 0.0f;
zmoutaou 0:70465b25c44f 404 }
zmoutaou 0:70465b25c44f 405
zmoutaou 0:70465b25c44f 406 // Apply feedback terms
zmoutaou 0:70465b25c44f 407 gx = gx + Kp * ex + Ki * eInt[0];
zmoutaou 0:70465b25c44f 408 gy = gy + Kp * ey + Ki * eInt[1];
zmoutaou 0:70465b25c44f 409 gz = gz + Kp * ez + Ki * eInt[2];
zmoutaou 0:70465b25c44f 410
zmoutaou 0:70465b25c44f 411 // Integrate rate of change of quaternion
zmoutaou 0:70465b25c44f 412 pa = q2;
zmoutaou 0:70465b25c44f 413 pb = q3;
zmoutaou 0:70465b25c44f 414 pc = q4;
zmoutaou 0:70465b25c44f 415 q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat);
zmoutaou 0:70465b25c44f 416 q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat);
zmoutaou 0:70465b25c44f 417 q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat);
zmoutaou 0:70465b25c44f 418 q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat);
zmoutaou 0:70465b25c44f 419
zmoutaou 0:70465b25c44f 420 // Normalise quaternion
zmoutaou 0:70465b25c44f 421 norm = sqrtf(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
zmoutaou 0:70465b25c44f 422 norm = 1.0f / norm;
zmoutaou 0:70465b25c44f 423 q[0] = q1 * norm;
zmoutaou 0:70465b25c44f 424 q[1] = q2 * norm;
zmoutaou 0:70465b25c44f 425 q[2] = q3 * norm;
zmoutaou 0:70465b25c44f 426 q[3] = q4 * norm;
zmoutaou 0:70465b25c44f 427 }
zmoutaou 0:70465b25c44f 428
zmoutaou 0:70465b25c44f 429
zmoutaou 0:70465b25c44f 430 int16_t readTempData()
zmoutaou 0:70465b25c44f 431 {
zmoutaou 0:70465b25c44f 432 uint8_t rawData[2]; // x/y/z gyro register data stored here
zmoutaou 0:70465b25c44f 433 readBytes(LSM9DS1XG_ADDRESS, LSM9DS1XG_OUT_TEMP_L, 2, &rawData[0]); // Read the two raw data registers sequentially into data array
zmoutaou 0:70465b25c44f 434 return (((int16_t)rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a 16-bit signed value
zmoutaou 0:70465b25c44f 435 }
zmoutaou 0:70465b25c44f 436 void initLSM9DS1()
zmoutaou 0:70465b25c44f 437 {
zmoutaou 0:70465b25c44f 438 // enable the 3-axes of the gyroscope
zmoutaou 0:70465b25c44f 439 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG4, 0x38);
zmoutaou 0:70465b25c44f 440 ////pc.printf( "enable the 3-axes of the gyroscope\n");
zmoutaou 0:70465b25c44f 441 // configure the gyroscope
zmoutaou 0:70465b25c44f 442 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG1_G, Godr << 5 | Gscale << 3 | Gbw);
zmoutaou 0:70465b25c44f 443 ////pc.printf("// configure the gyroscope\n");
zmoutaou 0:70465b25c44f 444 wait(0.2);
zmoutaou 0:70465b25c44f 445 // enable the three axes of the accelerometer
zmoutaou 0:70465b25c44f 446 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG5_XL, 0x38);
zmoutaou 0:70465b25c44f 447 ////pc.printf("// enable the three axes of the accelerometer\n ") ;
zmoutaou 0:70465b25c44f 448 // configure the accelerometer-specify bandwidth selection with Abw
zmoutaou 0:70465b25c44f 449 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG6_XL, Aodr << 5 | Ascale << 3 | 0x04 |Abw);
zmoutaou 0:70465b25c44f 450 ////pc.printf("configure the accelerometer-specify bandwidth selection with Abw\n");
zmoutaou 0:70465b25c44f 451 wait(0.2);
zmoutaou 0:70465b25c44f 452 // enable block data update, allow auto-increment during multiple byte read
zmoutaou 0:70465b25c44f 453 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG8, 0x44);
zmoutaou 0:70465b25c44f 454 ////pc.printf("enable block data update, allow auto-increment during multiple byte read\n");
zmoutaou 0:70465b25c44f 455 // configure the magnetometer-enable temperature compensation of mag data
zmoutaou 0:70465b25c44f 456 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG1_M, 0x80 | Mmode << 5 | Modr << 2); // select x,y-axis mode
zmoutaou 0:70465b25c44f 457 ////pc.printf("configure the magnetometer-enable temperature compensation of mag data\n");
zmoutaou 0:70465b25c44f 458 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG2_M, Mscale << 5 ); // select mag full scale
zmoutaou 0:70465b25c44f 459 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG3_M, 0x00 ); // continuous conversion mode
zmoutaou 0:70465b25c44f 460 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG4_M, Mmode << 2 ); // select z-axis mode
zmoutaou 0:70465b25c44f 461 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG5_M, 0x40 ); // select block update mode
zmoutaou 0:70465b25c44f 462 }
zmoutaou 0:70465b25c44f 463
zmoutaou 0:70465b25c44f 464 void selftestLSM9DS1()
zmoutaou 0:70465b25c44f 465 {
zmoutaou 0:70465b25c44f 466 float accel_noST[3] = {0., 0., 0.}, accel_ST[3] = {0., 0., 0.};
zmoutaou 0:70465b25c44f 467 float gyro_noST[3] = {0., 0., 0.}, gyro_ST[3] = {0., 0., 0.};
zmoutaou 0:70465b25c44f 468 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG10, 0x00); // disable self test
zmoutaou 0:70465b25c44f 469 accelgyrocalLSM9DS1(gyro_noST, accel_noST);
zmoutaou 0:70465b25c44f 470 ////pc.printf("disable self test ");
zmoutaou 0:70465b25c44f 471 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG10, 0x05); // enable gyro/accel self test
zmoutaou 0:70465b25c44f 472 accelgyrocalLSM9DS1(gyro_ST, accel_ST);
zmoutaou 0:70465b25c44f 473 ////pc.printf("enable gyro/accel self test");
zmoutaou 0:70465b25c44f 474
zmoutaou 0:70465b25c44f 475 float gyrodx = (gyro_ST[0] - gyro_noST[0]);
zmoutaou 0:70465b25c44f 476 float gyrody = (gyro_ST[1] - gyro_noST[1]);
zmoutaou 0:70465b25c44f 477 float gyrodz = (gyro_ST[2] - gyro_noST[2]);
zmoutaou 0:70465b25c44f 478
zmoutaou 0:70465b25c44f 479 ////pc.printf("Gyro self-test results: ");
zmoutaou 0:70465b25c44f 480 ////pc.printf("x-axis = %f , dps" ,gyrodx); //////////pc.printf(" should be between 20 and 250 dps \n");
zmoutaou 0:70465b25c44f 481 ////pc.printf("y-axis = %f ",gyrody); //////////pc.printf(" dps"); //////pc.printf(" should be between 20 and 250 dps \n");
zmoutaou 0:70465b25c44f 482 ////pc.printf("z-axis = %f ",gyrodz); //////////pc.printf(" dps"); //////pc.printf(" should be between 20 and 250 dps \n");
zmoutaou 0:70465b25c44f 483
zmoutaou 0:70465b25c44f 484 float accdx = 1000.*(accel_ST[0] - accel_noST[0]);
zmoutaou 0:70465b25c44f 485 float accdy = 1000.*(accel_ST[1] - accel_noST[1]);
zmoutaou 0:70465b25c44f 486 float accdz = 1000.*(accel_ST[2] - accel_noST[2]);
zmoutaou 0:70465b25c44f 487 //pc.printf( "%f,%f,%f,%f,%f,%f\n", gyrodx,gyrody,gyrodz,accdx,accdy,accdz);
zmoutaou 0:70465b25c44f 488 ////pc.printf("Accelerometer self-test results: ");
zmoutaou 0:70465b25c44f 489 ////pc.printf("x-axis = %f" , accdx); //////////pc.printf(" mg"); //////pc.printf(" should be between 60 and 1700 mg \n");
zmoutaou 0:70465b25c44f 490 ////pc.printf("y-axis = %f" , accdy); //////////pc.printf(" mg"); //////pc.printf(" should be between 60 and 1700 mg \n");
zmoutaou 0:70465b25c44f 491 ////pc.printf("z-axis = %f", accdz); ////////pc.printf(" mg"); //////pc.printf(" should be between 60 and 1700 mg \n");
zmoutaou 0:70465b25c44f 492
zmoutaou 0:70465b25c44f 493 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG10, 0x00); // disable self test
zmoutaou 0:70465b25c44f 494
zmoutaou 0:70465b25c44f 495 wait(0.2);
zmoutaou 0:70465b25c44f 496
zmoutaou 0:70465b25c44f 497 }
zmoutaou 0:70465b25c44f 498
zmoutaou 0:70465b25c44f 499 SDFileSystem sd(PA_7, PA_6, PA_5, PB_6, "sd");
zmoutaou 0:70465b25c44f 500 int main ()
zmoutaou 0:70465b25c44f 501 {
zmoutaou 0:70465b25c44f 502 FILE *fp= fopen("/sd/valeur_ble.txt", "a");
zmoutaou 0:70465b25c44f 503 wait(1);
zmoutaou 0:70465b25c44f 504 bool erreur; //pc.printf("LSM9DS1 9-axis motion sensor...\n");
zmoutaou 0:70465b25c44f 505 uint8_t c = readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_WHO_AM_I); // Read WHO_AM_I register for LSM9DS1 accel/gyro//pc.printf("LSM9DS1 accel/gyro % c ", c); ////////pc.printf("I AM %x",c ); ////////pc.printf(" I should be 0x68 \n");
zmoutaou 0:70465b25c44f 506 uint8_t d = 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:70465b25c44f 507 if (c == 0x68 && d == 0x3D) { // WHO_AM_I should always be 0x0E for the accel/gyro and 0x3C for the mag
zmoutaou 0:70465b25c44f 508 ////pc.printf("LSM9DS1 is online...\n"); // get sensor resolutions, only need to do this once
zmoutaou 0:70465b25c44f 509 getAres();////////pc.printf("accel sensitivity is %f .",1./(1000.*aRes)); //////////////pc.printf(" LSB/mg");
zmoutaou 0:70465b25c44f 510 getGres();////////pc.printf("gyro sensitivity is %f ." ,1./(1000.*gRes)); //////////////pc.printf(" LSB/mdps");
zmoutaou 0:70465b25c44f 511 getMres();////////pc.printf("mag sensitivity is %f .", 1./(1000.*mRes)); //////////////pc.printf(" LSB/mGauss");
zmoutaou 0:70465b25c44f 512 selftestLSM9DS1(); ////////pc.printf("Perform gyro and accel self test"); // check function of gyro and accelerometer via self test
zmoutaou 0:70465b25c44f 513 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:70465b25c44f 514 magcalLSM9DS1(magBias);////////pc.printf("mag biases (mG) %f %f %f \n",1000.*magBias[0],1000.*magBias[1],1000.*magBias[2]);
zmoutaou 0:70465b25c44f 515 wait(2); // add delay to see results before serial spew of data
zmoutaou 0:70465b25c44f 516 initLSM9DS1(); ////////pc.printf("LSM9DS1 initialized for active data mode...."); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
zmoutaou 0:70465b25c44f 517 erreur = false ;
zmoutaou 0:70465b25c44f 518 } else {
zmoutaou 0:70465b25c44f 519 erreur = true ;
zmoutaou 0:70465b25c44f 520 }
zmoutaou 0:70465b25c44f 521 t.start();
zmoutaou 0:70465b25c44f 522 while (erreur == false ) {
zmoutaou 0:70465b25c44f 523
zmoutaou 0:70465b25c44f 524 firstUpdate = t.read_ms();
zmoutaou 0:70465b25c44f 525 lastUpdate = firstUpdate;
zmoutaou 0:70465b25c44f 526 Now = t.read_ms();
zmoutaou 0:70465b25c44f 527 lastUpdate = Now;
zmoutaou 0:70465b25c44f 528
zmoutaou 0:70465b25c44f 529 if (readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_STATUS_REG) & 0x01) { //Accelerometer new data available. // check if new accel data is ready
zmoutaou 0:70465b25c44f 530 readAccelData(accelCount); // Read the x/y/z adc values // Now we'll calculate the accleration value into actual g's
zmoutaou 0:70465b25c44f 531 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
zmoutaou 0:70465b25c44f 532 ay = (float)accelCount[1]*aRes - accelBias[1];
zmoutaou 0:70465b25c44f 533 az = (float)accelCount[2]*aRes - accelBias[2];
zmoutaou 0:70465b25c44f 534 }
zmoutaou 0:70465b25c44f 535 if (readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_STATUS_REG) & 0x02) { // check if new gyro data is ready
zmoutaou 0:70465b25c44f 536 readGyroData(gyroCount); // Read the x/y/z adc values // Calculate the gyro value into actual degrees per second
zmoutaou 0:70465b25c44f 537 gx = (float)gyroCount[0]*gRes - gyroBias1[0]; // get actual gyro value, this depends on scale being set
zmoutaou 0:70465b25c44f 538 gy = (float)gyroCount[1]*gRes - gyroBias1[1];
zmoutaou 0:70465b25c44f 539 gz = (float)gyroCount[2]*gRes - gyroBias1[2];
zmoutaou 0:70465b25c44f 540 }
zmoutaou 0:70465b25c44f 541 if (readByte(LSM9DS1M_ADDRESS, LSM9DS1M_STATUS_REG_M) & 0x08) { // check if new mag data is ready
zmoutaou 0:70465b25c44f 542 readMagData(magCount); // Read the x/y/z adc values
zmoutaou 0:70465b25c44f 543 mx = (float)magCount[0]*mRes; // - magBias[0]; // get actual magnetometer value, this depends on scale being set
zmoutaou 0:70465b25c44f 544 my = (float)magCount[1]*mRes; // - magBias[1];
zmoutaou 0:70465b25c44f 545 mz = (float)magCount[2]*mRes; // - magBias[2];}
zmoutaou 0:70465b25c44f 546 }
zmoutaou 0:70465b25c44f 547 //MadgwickQuaternionUpdate( ay, ax, az, gy*PI/180.0f, gx*PI/180.0f, gz*PI/180.0f );
zmoutaou 0:70465b25c44f 548 int nowMs = t.read_ms();
zmoutaou 0:70465b25c44f 549 if( nowMs - lastPrint > printInterval ) {
zmoutaou 0:70465b25c44f 550 int nowMs = t.read_ms();
zmoutaou 0:70465b25c44f 551 if( nowMs - lastPrint > printInterval ) {
zmoutaou 0:70465b25c44f 552 lastPrint = nowMs;
zmoutaou 0:70465b25c44f 553 pc.printf("%d %f,%f,%f,%f,%f,%f\n",nowMs,ax,ay,az,gx,gy,gz);
zmoutaou 0:70465b25c44f 554 //pc.printf("gyro: %f %f %f\n\r", gx, gy, gz);
zmoutaou 0:70465b25c44f 555 //pc.printf("accel: %f %f %f\n\r", ax, ay, az);
zmoutaou 0:70465b25c44f 556 //pc.printf("quat q0 qx qy qz: %f %f %f %f\n\r", q[0], q[1], q[2], q[3]);
zmoutaou 0:70465b25c44f 557 //yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
zmoutaou 0:70465b25c44f 558 //pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
zmoutaou 0:70465b25c44f 559 //roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
zmoutaou 0:70465b25c44f 560 //pitch *= 180.0f / PI;
zmoutaou 0:70465b25c44f 561 //yaw *= 180.0f / PI;
zmoutaou 0:70465b25c44f 562 //yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04 ??????????????????????????????????????
zmoutaou 0:70465b25c44f 563 //roll *= 180.0f / PI;
zmoutaou 0:70465b25c44f 564 }
zmoutaou 0:70465b25c44f 565 }
zmoutaou 0:70465b25c44f 566 }
zmoutaou 0:70465b25c44f 567
zmoutaou 0:70465b25c44f 568 }