ENSMM
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
--- /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] ;
+}
+
+
+
+
+
+
+
+
+
+