ENSMM
Programme complet de bâton de marche sauf capteur cardiaque
Dependencies: mbed SimpleBLE X_NUCLEO_IDB0XA1 SDFileSystem MBed_Adafruit-GPS-Library Arduino USBDevice
main.cpp
- Committer:
- zmoutaou
- Date:
- 2020-01-27
- Revision:
- 0:6e330c197193
File content as of revision 0:6e330c197193:
//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] ;
}