Programme complet de bâton de marche sauf capteur cardiaque
Dependencies: mbed SimpleBLE X_NUCLEO_IDB0XA1 SDFileSystem MBed_Adafruit-GPS-Library Arduino USBDevice
Diff: main.cpp
- Revision:
- 0:6e330c197193
diff -r 000000000000 -r 6e330c197193 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Jan 27 12:04:30 2020 +0000 @@ -0,0 +1,831 @@ +//Includes + +//#include "mbed.h"//déja défini dans une bibliotheque +#include "SimpleBLE.h" +#include "USBSerial.h" +#include "SDFileSystem.h" +#include "Adafruit_SSD1306.h" +#include "Arduino.h" +#include "MBed_Adafruit_GPS.h" +#include "LSM9DS1_registre.h" + + + + +Timer dt ; +int t_avant = 0 ; +int t_m = 0 ; +int t_d = 0; +int t_apres = 0; + + + + +I2C i2c_m(PB_9, PB_8); +unsigned char nCRC; // calculated check sum to ensure PROM integrity +double dT, OFFSET, SENS, T2, OFFSET2, SENS2; // First order and second order corrections for raw S5637 temperature and pressure data +int16_t accelCount[3], gyroCount[3], magCount[3]; // Stores the 16-bit signed accelerometer, gyro, and mag sensor output +float gyroBias1[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}, magBias[3] = {0, 0, 0}; // Bias corrections for gyro, accelerometer, and magnetometer +int16_t tempCount; // temperature raw count output +float temperature; // Stores the LSM9DS1gyro internal chip temperature in degrees Celsius +double Temperature, Pressure; // stores MS5611 pressures sensor pressure and temperature +// global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System) +float GyroMeasError = PI * (40.0f / 180.0f); // gyroscope measurement error in rads/s (start at 40 deg/s) +float GyroMeasDrift = PI * (0.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) +// There is a tradeoff in the beta parameter between accuracy and response speed. +// In the original Madgwick study, beta of 0.041 (corresponding to GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy. +// However, with this value, the LSM9SD0 response time is about 10 seconds to a stable initial quaternion. +// Subsequent changes also require a longish lag time to a stable output, not fast enough for a quadcopter or robot car! +// By increasing beta (GyroMeasError) by about a factor of fifteen, the response time constant is reduced to ~2 sec +// I haven't noticed any reduction in solution accuracy. This is essentially the I coefficient in a PID control sense; +// the bigger the feedback coefficient, the faster the solution converges, usually at the expense of accuracy. +// In any case, this is the free parameter in the Madgwick filtering and fusion scheme. +float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta +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 +#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 +#define Ki 0.0f + +uint32_t delt_t = 0, count = 0, sumCount = 0; // used to control display output rate +float pitch, yaw, roll; +float deltat = 0.0f, sum = 0.0f; // integration interval for both filter schemes +uint32_t Now = 0; // used to calculate integration interval + +float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values +float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion +float eInt[3] = {0.0f, 0.0f, 0.0f}; // vector to hold integral error for Mahony meth + +// Specify sensor full scale +uint8_t Gscale = GFS_245DPS; // gyro full scale +uint8_t Godr = GODR_238Hz; // gyro data sample rate +uint8_t Gbw = GBW_med; // gyro data bandwidth +uint8_t Ascale = AFS_2G; // accel full scale +uint8_t Aodr = AODR_238Hz; // accel data sample rate +uint8_t Abw = ABW_50Hz; // accel data bandwidth +uint8_t Mscale = MFS_4G; // mag full scale +uint8_t Modr = MODR_10Hz; // mag data sample rate +uint8_t Mmode = MMode_HighPerformance; // magnetometer operation mode +float aRes, gRes, mRes; // scale resolutions per LSB for the sensors + + +void getMres(); +void getGres(); +void getAres(); +void writeByte(uint8_t address, uint8_t subAddress, uint8_t data); +uint8_t readByte(uint8_t address, uint8_t subAddress); +void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest); +void accelgyrocalLSM9DS1(float * dest1, float * dest2); +void magcalLSM9DS1(float * dest1); +void readAccelData(int16_t * destination); +void readGyroData(int16_t * destination); +void readMagData(int16_t * destination); +void initLSM9DS1(); +void selftestLSM9DS1(); + + +Serial * gps_Serial; +SDFileSystem sd(PA_7, PA_6, PA_5, PB_6, "sd"); +SimpleBLE ble("ObCP_Baton "); +FILE *fp_ble= fopen("/sd/valeur_ble.txt", "w"); +FILE *fp= fopen("/sd/valeur_force.txt", "w"); +I2C i2c(PB_9, PB_8); +Adafruit_SSD1306_I2c oled(i2c,PC_13); +USBSerial pc(0x1f00, 0x2012, 0x0001, false); + +AnalogIn tension_A502(A0); +DigitalOut myledr(PB_5); +DigitalOut myledg(PB_3); +DigitalOut myledb(PA_10); +InterruptIn button1(PB_4); +InterruptIn button2(PB_10); + + +float lat_dd, lon_dd, lat_1, lon_1, lat_2, lon_2, distance = 0; +float sum_distance = 0; +float steps = 0; +double avg_speed = 0; +float cal = 0; +int i_gps ; +float t =0; +float lastUpdate = 0; + +int height=0; +bool interruption1= false; +char name[20]; +int i=0 ; +int weight; +int age ; +char gender[1]; +int max_heart_rate; +bool message_envoyee= false; + + +const int size_data = 50; +float force_measure[size_data]; +float pression_measure[size_data]; +float volts_measure[size_data]; +int size_data_i= 0; +bool interruption2; +bool force_mesure = false; + +void fct_name_text(uint8_t newState); +void fct_height_int(uint8_t newState); +void fct_weight_int(uint8_t newState); +void fct_age_int(uint8_t newState); +void fct_gender_text(uint8_t newState); +void fct_max_heart_rate_int(uint8_t newState); +void fct_fini(uint8_t newState); + +void fct_interruption_ble(); + + +int intToStr(int x, char str[], int d); +void ftoa(float n, char* res, int afterpoint); +float pow_nc(float n, int k); +void reverse(char* str, int len) ; + +void fct_interruption_force(void); + +void OLED_writeString(string str, int x, int y); +void OLED_drawSmile(); + + + +SimpleChar<uint8_t> name_text = ble.writeOnly_u8(0x8600, 0x8601, &fct_name_text); +SimpleChar<uint8_t> height_int = ble.writeOnly_u8(0x8600, 0x8602, &fct_height_int); +SimpleChar<uint8_t> weight_int= ble.writeOnly_u8(0x8600, 0x8603, &fct_weight_int); +SimpleChar<uint8_t> age_int= ble.writeOnly_u8(0x8600, 0x8604, &fct_age_int); +SimpleChar<uint8_t> gender_text= ble.writeOnly_u8(0x8600, 0x8605, &fct_gender_text); +SimpleChar<uint8_t> max_heart_rate_u8= ble.writeOnly_u8(0x8600, 0x8606, &fct_max_heart_rate_int); +SimpleChar<uint8_t> fini = ble.writeOnly_u8(0x8600, 0x8607, &fct_fini); + +int main(int, char**) +{ + bool erreur_imu = false; + ble.start(); + gps_Serial = new Serial(PA_2,PA_3); //serial object for use w/ GPS + Adafruit_GPS myGPS(gps_Serial); //object of Adafruit's GPS class + char c; //when read via Adafruit_GPS::read(), the class returns single character stored here + Timer refresh_Timer; //sets up a timer for use in loop; how often do we print GPS info? + const int refresh_Time = 2000; //refresh time in ms + button1.rise(&fct_interruption_ble); + button2.rise(&fct_interruption_force); + uint8_t c_who_I_m = readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_WHO_AM_I); + 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"); + if (c_who_I_m == 0x68 && d_who_I_m == 0x3D) { + // WHO_AM_I should always be 0x0E for the accel/gyro and 0x3C for the mag + getAres();////////pc.printf("accel sensitivity is %f .",1./(1000.*aRes)); //////////////pc.printf(" LSB/mg"); + getGres();////////pc.printf("gyro sensitivity is %f ." ,1./(1000.*gRes)); //////////////pc.printf(" LSB/mdps"); + getMres();////////pc.printf("mag sensitivity is %f .", 1./(1000.*mRes)); //////////////pc.printf(" LSB/mGauss"); + selftestLSM9DS1(); ////////pc.printf("Perform gyro and accel self test"); // check function of gyro and accelerometer via self test + 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]); + magcalLSM9DS1(magBias);////////pc.printf("mag biases (mG) %f %f %f \n",1000.*magBias[0],1000.*magBias[1],1000.*magBias[2]); + wait(2); // add delay to see results before serial spew of data + initLSM9DS1(); ////////pc.printf("LSM9DS1 initialized for active data mode...."); // Initialize device for active mode read of acclerometer, gyroscope, and temperature + erreur_imu = true; + } + myGPS.begin(9600); + myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA); //these commands are defined in MBed_Adafruit_GPS.h; a link is provided there for command creation + myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ); + myGPS.sendCommand(PGCMD_ANTENNA); + oled.clearDisplay(); + OLED_writeString("Hello! ", 32, 16); + pc.printf("\n********************************Bonjour**************************************\n"); + refresh_Timer.start(); + while (1) { + while ( interruption1 == true) { + myledr =1 ; + if ( message_envoyee== true ) { + wait(1); + fprintf(fp,"\n********************************BLE**************************************\n"); + fprintf(fp,"name: %s ; ",name); + fprintf(fp,"height: %d ;",height); + fprintf(fp,"weight: %d ;",weight); + fprintf(fp,"age: %d ;",age); + fprintf(fp,"gender: %s ;",gender); + fprintf(fp,"max_heart_rate: %d ;",max_heart_rate); + fprintf(fp,"\n********************************BLE-fin**************************************\n"); + message_envoyee= false; + myledr =0 ; + interruption1= false; + oled.clearDisplay(); + OLED_writeString(" welcome ", 32, 16); + oled.clearDisplay(); + OLED_writeString(name, 32, 16); + } else { + ble.waitForEvent(); + } + } + + while ( (interruption2 == true) and (force_mesure ==false )) { + myledg =1; + if (size_data_i==0 ) { + fprintf(fp,"\n volts_measure pression_measure force_measure \n"); + } + volts_measure[size_data_i] = tension_A502.read()*3.3 ; + 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; + force_measure[size_data_i] = pression_measure[size_data_i] *0.00193/9.81; + if( not (pression_measure[size_data_i]< 0 or force_measure[size_data_i]< 0) ) { + fprintf(fp,"%f %f %f \n",volts_measure[size_data_i],pression_measure[size_data_i],force_measure[size_data_i] ); + pc.printf("%f %f %f \n",volts_measure[size_data_i],pression_measure[size_data_i],force_measure[size_data_i] ); + oled.clearDisplay(); + oled.setTextSize(2); + char res[20]; + ftoa(force_measure[size_data_i], res, 3); + OLED_writeString(res, 1, 1); + size_data_i =size_data_i+1; + wait(0.5); + if (size_data_i ==size_data-1 ) { + interruption2 = false; + force_mesure = true ; + size_data_i =0; + myledg =0; + } + + } + + } + if ( force_mesure ==true ) { + oled.clearDisplay(); + while (1) { + dt.start(); + if (erreur_imu == true) { + int l = 0; + fprintf(fp,"\n********************************Gait **************************************\n"); + fprintf(fp,"ax, ay, az, gx, gy,gz,mx, my,mz\n"); + while (l<5000) { + + if (readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_STATUS_REG) & 0x01) { + //Accelerometer new data available. // check if new accel data is ready + readAccelData(accelCount); // Read the x/y/z adc values // Now we'll calculate the accleration value into actual g's + ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set + ay = (float)accelCount[1]*aRes - accelBias[1]; + az = (float)accelCount[2]*aRes - accelBias[2]; + } + if (readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_STATUS_REG) & 0x02) { // check if new gyro data is ready + readGyroData(gyroCount); // Read the x/y/z adc values // Calculate the gyro value into actual degrees per second + gx = (float)gyroCount[0]*gRes - gyroBias1[0]; // get actual gyro value, this depends on scale being set + gy = (float)gyroCount[1]*gRes - gyroBias1[1]; + gz = (float)gyroCount[2]*gRes - gyroBias1[2]; + } + + if (readByte(LSM9DS1M_ADDRESS, LSM9DS1M_STATUS_REG_M) & 0x08) { // check if new mag data is ready + readMagData(magCount); // Read the x/y/z adc values + mx = (float)magCount[0]*mRes; // - magBias[0]; // get actual magnetometer value, this depends on scale being set + my = (float)magCount[1]*mRes; // - magBias[1]; + mz = (float)magCount[2]*mRes; // - magBias[2];} + } + l= l+1; + fprintf(fp,"\n %f %f %f ;%f %f %f ;%f %f %f ", ax, ay, az, gx, gy,gz,mx, my,mz); + wait_us(1); + } + } else + pc.printf("Erreur LSM9DS1"); + } + fprintf(fp,"\n********************************GPS **************************************\n"); + c = myGPS.read(); //queries the GPS + myledb = 0; + if (c) { } + if ( myGPS.newNMEAreceived() ) { + if ( !myGPS.parse(myGPS.lastNMEA())) { + continue; + } + } + if (refresh_Timer.read_ms() >= refresh_Time) { + refresh_Timer.reset(); + + if (myGPS.fix) { + myledb = 0.5; + fprintf(fp,"Time: %d:%d:%d.%u\n", myGPS.hour+1, myGPS.minute, myGPS.seconds, myGPS.milliseconds); + Now = millis()*0.001 ; + t = Now - t; + fprintf(fp,"Date: %d/%d/20%d\n", myGPS.day, myGPS.month, myGPS.year); + fprintf(fp,"Fix:. %d\n", (int) myGPS.fix); + fprintf(fp,"Quality: %d\n\n", (int) myGPS.fixquality); + + pc.printf("Time: %d:%d:%d.%u\n", myGPS.hour+1, myGPS.minute, myGPS.seconds, myGPS.milliseconds); + pc.printf("Date: %d/%d/20%d\n", myGPS.day, myGPS.month, myGPS.year); + pc.printf("Fix: %d\n", (int) myGPS.fix); + pc.printf("Quality: %d\n\n", (int) myGPS.fixquality); + lat_dd = myGPS.coordToDegDec(myGPS.latitude); + lon_dd = myGPS.coordToDegDec(myGPS.longitude); + if (i_gps == 0) { + lat_1 = lat_dd; + lon_1 = lon_dd; + lat_2 = lat_1; + lon_2 = lon_1; + t_m = dt.read_ms() ; + t_m = dt.read(); + } else { + lat_1 = lat_2; + lon_1 = lon_2; + lat_2 = lat_dd; + lon_2 = lon_dd; + t_apres = t_m ; + t_m = dt.read(); + t_d = t_apres - t_m ; + ::distance = myGPS.getDistance(lat_1, lon_1, lat_2, lon_2); + ::sum_distance += ::distance; + ::steps = myGPS.getSteps(::sum_distance, height); + ::avg_speed = myGPS.getAvgSpeed(::sum_distance, t); + char res[20]; + ftoa(steps, res, 1); + OLED_writeString(res, 1, 1); + } + fprintf(fp,"Satellites : %d\n", myGPS.satellites); + fprintf(fp,"Location (deg) : %5.5f%c, %5.5f%c\n", lat_dd, myGPS.lat, lon_dd, myGPS.lon); + fprintf(fp,"Altitude (m) : %5.2f\n", myGPS.altitude); + fprintf(fp,"Distance (m) : %5.2f\n", ::sum_distance); + fprintf(fp,"Steps taken : %5.0f\n", ::steps); + fprintf(fp,"Average speed (km/h) : %5.2f\n\n\n", ::avg_speed); + pc.printf("Satellites : %d\n", myGPS.satellites); + pc.printf("Location (deg) : %5.5f%c, %5.5f%c\n", lat_dd, myGPS.lat, lon_dd, myGPS.lon); + pc.printf("Altitude (m) : %5.2f\n", myGPS.altitude); + pc.printf("Distance (m) : %5.2f\n", ::sum_distance); + pc.printf("Steps taken : %5.0f\n", ::steps); + pc.printf("Average speed (km/h) : %5.5f\n\n\n", ::avg_speed); + myledb = 0; + i_gps++; + } + } + } // Closing the file + } +} + + + + + +void fct_name_text(uint8_t newState) +{ + name[i] = (char) newState; + i= i+1 ; + pc.printf("name %s ",name); +} +void fct_height_int(uint8_t newState) +{ + height = (int) newState; + pc.printf("height %d ",height); +} +void fct_weight_int(uint8_t newState) +{ + weight = (int) newState; + pc.printf("weight %d ",weight); +} +void fct_age_int(uint8_t newState) +{ + age = (int) newState; + pc.printf("age %d ",age); +} +void fct_gender_text(uint8_t newState) +{ + gender[0] = (char) newState; + pc.printf("gender %s ",gender); +} +void fct_max_heart_rate_int(uint8_t newState) +{ + max_heart_rate = (int) newState; + pc.printf("max_heart_rate %d ",max_heart_rate); +} +void fct_fini(uint8_t newState) +{ + message_envoyee= true; + pc.printf("message_envoyee "); +} + +void fct_interruption_ble() +{ + interruption1= true ; + height = 0; + i=0; + for (int j=1; j<sizeof(name); j++) { + name[j] = ' '; + } + weight=0; + age =0; + char gender[1] = {'_'} ; + max_heart_rate=0; + message_envoyee= false; +} + +void fct_interruption_force() +{ + interruption2 = true ; + force_mesure = false; +} + + +void OLED_writeString(string str, int x, int y) +{ + oled.setTextCursor(x, y); + oled.fillRect(x, y, 128, 8, 0); + + for (int k = 0; k < str.length(); k++) { + oled.writeChar(str[k]); + } + + oled.display(); +} + +void ftoa(float n, char* res, int afterpoint) //https://www.geeksforgeeks.org/convert-floating-point-number-string/ +{ + + int ipart = (int)n; // Extract integer part + float fpart = n - (float)ipart; // Extract floating part + int k = intToStr(ipart, res, 0); // convert integer part to string + if (afterpoint != 0) { // check for display option after point + res[k] = '.'; // a + fpart = fpart * pow_nc(10, afterpoint); + intToStr((int)fpart, res + k + 1, afterpoint); + } +} + +int intToStr(int x, char str[], int d) +{ + int k = 0; + while (x) { + str[k++] = (x % 10) + '0'; + x = x / 10; + } + + // If number of digits required is more, then + // add 0s at the beginning + while (k < d) + str[k++] = '0'; + + reverse(str, k); + str[k] = '\0'; + return k; +} + + +void reverse(char* str, int len) +{ + int k = 0, j = len - 1, temp; + while (k < j) { + temp = str[k]; + str[k] = str[j]; + str[j] = temp; + k++; + j--; + } +} + + +float pow_nc(float n, int l) +{ + for (int k=1; k<l; k++) { + n= n*n ; + } + return(n); +} + + +// Mostra no display uma mensagem de receptividade +void OLED_drawSmile() +{ + oled.clearDisplay(); + oled.drawCircle(12, 20, 9, 1); + oled.fillRect(0, 5, 22, 15, 0); + oled.fillTriangle(2, 15, 10, 15, 6, 7, 1); + oled.fillTriangle(3, 15, 9, 15, 6, 8, 0); + oled.fillTriangle(14, 15, 22, 15, 18, 7, 1); + oled.fillTriangle(15, 15, 21, 15, 18, 8, 0); + oled.clearDisplay(); + +} +void selftestLSM9DS1() +{ + float accel_noST[3] = {0., 0., 0.}, accel_ST[3] = {0., 0., 0.}; + float gyro_noST[3] = {0., 0., 0.}, gyro_ST[3] = {0., 0., 0.}; + writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG10, 0x00); // disable self test + accelgyrocalLSM9DS1(gyro_noST, accel_noST); + ////pc.printf("disable self test "); + writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG10, 0x05); // enable gyro/accel self test + accelgyrocalLSM9DS1(gyro_ST, accel_ST); + ////pc.printf("enable gyro/accel self test"); + + float gyrodx = (gyro_ST[0] - gyro_noST[0]); + float gyrody = (gyro_ST[1] - gyro_noST[1]); + float gyrodz = (gyro_ST[2] - gyro_noST[2]); + + ////pc.printf("Gyro self-test results: "); + ////pc.printf("x-axis = %f , dps" ,gyrodx); //////////pc.printf(" should be between 20 and 250 dps \n"); + ////pc.printf("y-axis = %f ",gyrody); //////////pc.printf(" dps"); //////pc.printf(" should be between 20 and 250 dps \n"); + ////pc.printf("z-axis = %f ",gyrodz); //////////pc.printf(" dps"); //////pc.printf(" should be between 20 and 250 dps \n"); + + float accdx = 1000.*(accel_ST[0] - accel_noST[0]); + float accdy = 1000.*(accel_ST[1] - accel_noST[1]); + float accdz = 1000.*(accel_ST[2] - accel_noST[2]); + //pc.printf( "%f,%f,%f,%f,%f,%f\n", gyrodx,gyrody,gyrodz,accdx,accdy,accdz); + ////pc.printf("Accelerometer self-test results: "); + ////pc.printf("x-axis = %f" , accdx); //////////pc.printf(" mg"); //////pc.printf(" should be between 60 and 1700 mg \n"); + ////pc.printf("y-axis = %f" , accdy); //////////pc.printf(" mg"); //////pc.printf(" should be between 60 and 1700 mg \n"); + ////pc.printf("z-axis = %f", accdz); ////////pc.printf(" mg"); //////pc.printf(" should be between 60 and 1700 mg \n"); + + writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG10, 0x00); // disable self test + + wait(0.2); + +} + + + +void initLSM9DS1() +{ + // enable the 3-axes of the gyroscope + writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG4, 0x38); + ////pc.printf( "enable the 3-axes of the gyroscope\n"); + // configure the gyroscope + writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG1_G, Godr << 5 | Gscale << 3 | Gbw); + ////pc.printf("// configure the gyroscope\n"); + wait(0.2); + // enable the three axes of the accelerometer + writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG5_XL, 0x38); + ////pc.printf("// enable the three axes of the accelerometer\n ") ; + // configure the accelerometer-specify bandwidth selection with Abw + writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG6_XL, Aodr << 5 | Ascale << 3 | 0x04 |Abw); + ////pc.printf("configure the accelerometer-specify bandwidth selection with Abw\n"); + wait(0.2); + // enable block data update, allow auto-increment during multiple byte read + writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG8, 0x44); + ////pc.printf("enable block data update, allow auto-increment during multiple byte read\n"); + // configure the magnetometer-enable temperature compensation of mag data + writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG1_M, 0x80 | Mmode << 5 | Modr << 2); // select x,y-axis mode + ////pc.printf("configure the magnetometer-enable temperature compensation of mag data\n"); + writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG2_M, Mscale << 5 ); // select mag full scale + writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG3_M, 0x00 ); // continuous conversion mode + writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG4_M, Mmode << 2 ); // select z-axis mode + writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG5_M, 0x40 ); // select block update mode +} +void getMres() + +{ + switch (Mscale) { + // Possible magnetometer scales (and their register bit settings) are: + // 4 Gauss (00), 8 Gauss (01), 12 Gauss (10) and 16 Gauss (11) + case MFS_4G: + mRes = 4.0/32768.0; + break; + case MFS_8G: + mRes = 8.0/32768.0; + break; + case MFS_12G: + mRes = 12.0/32768.0; + break; + case MFS_16G: + mRes = 16.0/32768.0; + break; + } +} + + +void getGres() + +{ + switch (Gscale) { + // Possible gyro scales (and their register bit settings) are: + // 245 DPS (00), 500 DPS (01), and 2000 DPS (11). + case GFS_245DPS: + gRes = 245.0/32768.0;//2^15 + break; + case GFS_500DPS: + gRes = 500.0/32768.0; + break; + case GFS_2000DPS: + gRes = 2000.0/32768.0; + break; + } +} + + +void getAres() + +{ + switch (Ascale) { + // Possible accelerometer scales (and their register bit settings) are: + // 2 Gs (00), 16 Gs (01), 4 Gs (10), and 8 Gs (11). + case AFS_2G: + aRes = 2.0/32768.0; + break; + case AFS_16G: + aRes = 16.0/32768.0; + break; + case AFS_4G: + aRes = 4.0/32768.0; + break; + case AFS_8G: + aRes = 8.0/32768.0; + break; + } +} + +void writeByte(uint8_t address, uint8_t subAddress, uint8_t data) + +{ + char temp_data[2] = {subAddress, data}; + i2c_m.write(address, temp_data, 2); +} +uint8_t readByte(uint8_t address, uint8_t subAddress) + +{ + char data; + char temp[1] = {subAddress}; + i2c_m.write(address, temp, 1); + temp[1] = 0x00; + i2c_m.write(address, temp, 1); + int a = i2c_m.read(address, &data, 1); + return data; + +} +void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest) + +{ + + int i; + char temp_dest[count]; + char temp[1] = {subAddress}; + i2c_m.write(address, temp, 1); + i2c_m.read(address, temp_dest, count); + for (i=0; i < count; i++) { + dest[i] = temp_dest[i]; + } +} + + +void accelgyrocalLSM9DS1(float * dest1, float * dest2) + +{ + uint8_t data[6] = {0}; + int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0}; + uint16_t samples, ii; + writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG4, 0x38); // enable the 3-axes of the gyroscope + writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG1_G, Godr << 5 | Gscale << 3 | Gbw); // configure the gyroscope + wait(0.2); + writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG5_XL, 0x38); // enable the three axes of the accelerometer + writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG6_XL, Aodr << 5 | Ascale << 3 | 0x04 |Abw); // configure the accelerometer-specify bandwidth selection with Abw + wait(0.2); + writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG8, 0x44);// enable block data update, allow auto-increment during multiple byte read + uint8_t c = readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9);// First get gyro bias + writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9, (c & 0xFD)| 0x02); // Enable gyro FIFO + wait(0.050); // Wait for change to take effect + writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_CTRL, 0x20 | 0x1F); // Enable gyro FIFO stream mode and set watermark at 32 samples + wait(1); // delay 1000 milliseconds to collect FIFO samples + samples = (readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_SRC) & 0x2F); // Read number of stored samples + for(ii = 0; ii < samples ; ii++) { + // Read the gyro data stored in the FIFO + int16_t gyro_temp[3] = {0, 0, 0}; + readBytes(LSM9DS1XG_ADDRESS, LSM9DS1XG_OUT_X_L_G, 6, &data[0]); + gyro_temp[0] = (int16_t) (((int16_t)data[1] << 8) | data[0]); // Form signed 16-bit integer for each sample in FIFO + gyro_temp[1] = (int16_t) (((int16_t)data[3] << 8) | data[2]); + gyro_temp[2] = (int16_t) (((int16_t)data[5] << 8) | data[4]); + + gyro_bias[0] += (int32_t) gyro_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases + gyro_bias[1] += (int32_t) gyro_temp[1]; + gyro_bias[2] += (int32_t) gyro_temp[2]; + } + + gyro_bias[0] /= samples; // average the data + gyro_bias[1] /= samples; + gyro_bias[2] /= samples; + + dest1[0] = (float)gyro_bias[0]*gRes; // Properly scale the data to get deg/s + dest1[1] = (float)gyro_bias[1]*gRes; + dest1[2] = (float)gyro_bias[2]*gRes; + + c = readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9); + writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9, c & ~0x02); //Disable gyro FIFO + wait(0.050); + writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_CTRL, 0x00); // Enable gyro bypass mode + + // now get the accelerometer bias + c = readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9); + writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9, (c & 0xFD)| 0x02); // Enable accel FIFO + wait(0.050); // Wait for change to take effect + writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_CTRL, 0x20 | 0x1F); // Enable accel FIFO stream mode and set watermark at 32 samples + wait(1); // delay 1000 milliseconds to collect FIFO samples + + samples = (readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_SRC) & 0x2F); // Read number of stored samples + + for(ii = 0; ii < samples ; ii++) { // Read the accel data stored in the FIFO + int16_t accel_temp[3] = {0, 0, 0}; + readBytes(LSM9DS1XG_ADDRESS, LSM9DS1XG_OUT_X_L_XL, 6, &data[0]); + accel_temp[0] = (int16_t) (((int16_t)data[1] << 8) | data[0]); // Form signed 16-bit integer for each sample in FIFO + accel_temp[1] = (int16_t) (((int16_t)data[3] << 8) | data[2]); + accel_temp[2] = (int16_t) (((int16_t)data[5] << 8) | data[4]); + + accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases + accel_bias[1] += (int32_t) accel_temp[1]; + accel_bias[2] += (int32_t) accel_temp[2]; + } + + accel_bias[0] /= samples; // average the data + accel_bias[1] /= samples; + accel_bias[2] /= samples; + + if(accel_bias[2] > 0L) { + accel_bias[2] -= (int32_t) (1.0/aRes); // Remove gravity from the z-axis accelerometer bias calculation + } else { + accel_bias[2] += (int32_t) (1.0/aRes); + } + + dest2[0] = (float)accel_bias[0]*aRes; // Properly scale the data to get g + dest2[1] = (float)accel_bias[1]*aRes; + dest2[2] = (float)accel_bias[2]*aRes; + + c = readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9); + writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9, c & ~0x02); //Disable accel FIFO + wait(0.050); + writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_CTRL, 0x00); // Enable accel bypass mode*/ +} +void magcalLSM9DS1(float * dest1) + +{ + uint8_t data[6]; // data array to hold mag x, y, z, data + uint16_t ii = 0, sample_count = 0; + int32_t mag_bias[3] = {0, 0, 0}; + int16_t mag_max[3] = {0, 0, 0}, mag_min[3] = {0, 0, 0}; + + // configure the magnetometer-enable temperature compensation of mag data + writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG1_M, 0x80 | Mmode << 5 | Modr << 2); // select x,y-axis mode + writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG2_M, Mscale << 5 ); // select mag full scale + writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG3_M, 0x00 ); // continuous conversion mode + writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG4_M, Mmode << 2 ); // select z-axis mode + writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG5_M, 0x40 ); // select block update mode + ////pc.printf("Mag Calibration: Wave device in a figure eight until done!\n"); + wait(0.004); + + sample_count = 128; + for(ii = 0; ii < sample_count; ii++) { + int16_t mag_temp[3] = {0, 0, 0}; + readBytes(LSM9DS1M_ADDRESS, LSM9DS1M_OUT_X_L_M, 6, &data[0]); // Read the six raw data registers into data array + mag_temp[0] = (int16_t) (((int16_t)data[1] << 8) | data[0]) ; // Form signed 16-bit integer for each sample in FIFO + mag_temp[1] = (int16_t) (((int16_t)data[3] << 8) | data[2]) ; + mag_temp[2] = (int16_t) (((int16_t)data[5] << 8) | data[4]) ; + for (int jj = 0; jj < 3; jj++) { + if(mag_temp[jj] > mag_max[jj]) mag_max[jj] = mag_temp[jj]; + if(mag_temp[jj] < mag_min[jj]) mag_min[jj] = mag_temp[jj]; + } + wait(0.105); // at 10 Hz ODR, new mag data is available every 100 ms + } + +// ////pc.printf("mag x min/max:"); //pc.printfmag_max[0]); //pc.printfmag_min[0]); +// ////pc.printf("mag y min/max:"); //pc.printfmag_max[1]); //pc.printfmag_min[1]); +// ////pc.printf("mag z min/max:"); //pc.printfmag_max[2]); //pc.printfmag_min[2]); + + mag_bias[0] = (mag_max[0] + mag_min[0])/2; // get average x mag bias in counts + mag_bias[1] = (mag_max[1] + mag_min[1])/2; // get average y mag bias in counts + mag_bias[2] = (mag_max[2] + mag_min[2])/2; // get average z mag bias in counts + + dest1[0] = (float) mag_bias[0]*mRes; // save mag biases in G for main program + dest1[1] = (float) mag_bias[1]*mRes; + dest1[2] = (float) mag_bias[2]*mRes; + + //write biases to accelerometermagnetometer offset registers as counts); + writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_OFFSET_X_REG_L_M, (int16_t) mag_bias[0] & 0xFF); + writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_OFFSET_X_REG_H_M, ((int16_t)mag_bias[0] >> 8) & 0xFF); + writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_OFFSET_Y_REG_L_M, (int16_t) mag_bias[1] & 0xFF); + writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_OFFSET_Y_REG_H_M, ((int16_t)mag_bias[1] >> 8) & 0xFF); + writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_OFFSET_Z_REG_L_M, (int16_t) mag_bias[2] & 0xFF); + writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_OFFSET_Z_REG_H_M, ((int16_t)mag_bias[2] >> 8) & 0xFF); + + ////pc.printf("Mag Calibration done!\n"); +} + +void readAccelData(int16_t * destination) + +{ + uint8_t rawData[6]; // x/y/z accel register data stored here + readBytes(LSM9DS1XG_ADDRESS, LSM9DS1XG_OUT_X_L_XL, 6, &rawData[0]); // Read the six raw data registers into data array + destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value + destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ; + destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; +} + +void readGyroData(int16_t * destination) + +{ + uint8_t rawData[6]; // x/y/z gyro register data stored here + readBytes(LSM9DS1XG_ADDRESS, LSM9DS1XG_OUT_X_L_G, 6, &rawData[0]); // Read the six raw data registers sequentially into data array + destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value + destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ; + destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; +} + +void readMagData(int16_t * destination) +{ + uint8_t rawData[6]; // x/y/z gyro register data stored here + readBytes(LSM9DS1M_ADDRESS, LSM9DS1M_OUT_X_L_M, 6, &rawData[0]); // Read the six raw data registers sequentially into data array + destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value + destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ; // Data stored as little Endian + destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; +} + + + + + + + + + +