![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Example from X-NUCLEO-IKS01A3 to calibrate Magnetometer and use to read compass heading degrees without declination
Dependencies: X_NUCLEO_IKS01A3
Revision 9:3486ba668395, committed 2022-06-14
- Comitter:
- teachloris
- Date:
- Tue Jun 14 20:20:06 2022 +0000
- Parent:
- 8:f9a18e31c9d4
- Commit message:
- Example calibration and read magnetometer
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r f9a18e31c9d4 -r 3486ba668395 main.cpp --- a/main.cpp Thu Oct 29 12:57:19 2020 +0000 +++ b/main.cpp Tue Jun 14 20:20:06 2022 +0000 @@ -41,113 +41,91 @@ #include "rtos.h" #include "XNucleoIKS01A3.h" +#define PI 3.14159 +int32_t minx,miny,minz,maxx,maxy,maxz; + + /* Instantiate the expansion board */ -static XNucleoIKS01A3 *mems_expansion_board = XNucleoIKS01A3::instance(D14, D15, D4, D5, A3, D6, A4); +static XNucleoIKS01A3 *mems_expansion_board = XNucleoIKS01A3::instance(D14, D15);//, D4, D5, A3, D6, A4); /* Retrieve the composing elements of the expansion board */ static LIS2MDLSensor *magnetometer = mems_expansion_board->magnetometer; -static HTS221Sensor *hum_temp = mems_expansion_board->ht_sensor; -static LPS22HHSensor *press_temp = mems_expansion_board->pt_sensor; -static LSM6DSOSensor *acc_gyro = mems_expansion_board->acc_gyro; -static LIS2DW12Sensor *accelerometer = mems_expansion_board->accelerometer; -static STTS751Sensor *temp = mems_expansion_board->t_sensor; + -/* Helper function for printing floats & doubles */ -static char *print_double(char *str, double v, int decimalDigits = 2) +void calibra() { - int i = 1; - int intPart, fractPart; - int len; - char *ptr; - - /* prepare decimal digits multiplicator */ - for (; decimalDigits != 0; i *= 10, decimalDigits--); - - /* calculate integer & fractinal parts */ - intPart = (int)v; - fractPart = (int)((v - (double)(int)v) * i); + int32_t axes[3]; - /* fill in integer part */ - sprintf(str, "%i.", intPart); - - /* prepare fill in of fractional part */ - len = strlen(str); - ptr = &str[len]; - - /* fill in leading fractional zeros */ - for (i /= 10; i > 1; i /= 10, ptr++) { - if (fractPart >= i) { - break; - } - *ptr = '0'; + magnetometer->get_m_axes(axes); + + //setto tutti i valori alla prima lettura + minx=axes[0]; + miny=axes[1]; + minz=axes[2]; + maxx=minx; + maxy=miny; + maxz=minz; + //imposto un timer per leggere per 10 secondi + Timer t; + t.start(); + while(t.read()<10){ + + magnetometer->get_m_axes(axes); + + if (minx>axes[0])minx=axes[0]; + if (miny>axes[1])miny=axes[1]; + if (minz>axes[2])minz=axes[2]; + if (maxx<axes[0])maxx=axes[0]; + if (maxy<axes[1])maxy=axes[1]; + if (maxz<axes[2])maxz=axes[2]; } - - /* fill in (rest of) fractional part */ - sprintf(ptr, "%i", fractPart); - - return str; + t.stop(); + printf("LIS2MDL [mag/mgauss]: minx %6d, maxx %6d, miny %6d maxy %6d\r\n", minx,maxx,miny,maxy); + } - +void normalizza(int32_t axes[]){ + int32_t max,min; + + if (minx>axes[0])max=minx; + else max=axes[0]; + if (max<maxx)min=max; + else min=maxx; + axes[0]=(min-minx)*200/(maxx-minx)-100; + + if (miny>axes[1])max=miny; + else max=axes[1]; + if (max<maxy)min=max; + else min=maxy; + axes[1]=(min-miny)*200/(maxy-miny)-100; + + } + /* Simple main function */ int main() { uint8_t id; - float value1, value2; - char buffer1[32], buffer2[32]; + //float value1, value2; + //char buffer1[32], buffer2[32]; int32_t axes[3]; - + int direzione=0; + /* Enable all sensors */ - hum_temp->enable(); - press_temp->enable(); - temp->enable(); magnetometer->enable(); - accelerometer->enable_x(); - acc_gyro->enable_x(); - acc_gyro->enable_g(); - - printf("\r\n--- Starting new run ---\r\n"); - - hum_temp->read_id(&id); - printf("HTS221 humidity & temperature = 0x%X\r\n", id); - press_temp->read_id(&id); - printf("LPS22HH pressure & temperature = 0x%X\r\n", id); - temp->read_id(&id); - printf("STTS751 temperature = 0x%X\r\n", id); magnetometer->read_id(&id); - printf("LIS2MDL magnetometer = 0x%X\r\n", id); - accelerometer->read_id(&id); - printf("LIS2DW12 accelerometer = 0x%X\r\n", id); - acc_gyro->read_id(&id); - printf("LSM6DSO accelerometer & gyroscope = 0x%X\r\n", id); - + calibra(); + + // magnetometer->get_m_axes(axes); + // printf("LIS2MDL [mag/mgauss]: %6d, %6d, %6d\r\n", axes[0], axes[1], axes[2]); + while (1) { - printf("\r\n"); - - hum_temp->get_temperature(&value1); - hum_temp->get_humidity(&value2); - printf("HTS221: [temp] %7s C, [hum] %s%%\r\n", print_double(buffer1, value1), print_double(buffer2, value2)); - - press_temp->get_temperature(&value1); - press_temp->get_pressure(&value2); - printf("LPS22HH: [temp] %7s C, [press] %s mbar\r\n", print_double(buffer1, value1), print_double(buffer2, value2)); - - temp->get_temperature(&value1); - printf("STTS751: [temp] %7s C\r\n", print_double(buffer1, value1)); - - printf("---\r\n"); - + magnetometer->get_m_axes(axes); - printf("LIS2MDL [mag/mgauss]: %6d, %6d, %6d\r\n", axes[0], axes[1], axes[2]); - - accelerometer->get_x_axes(axes); - printf("LIS2DW12 [acc/mg]: %6d, %6d, %6d\r\n", axes[0], axes[1], axes[2]); - - acc_gyro->get_x_axes(axes); - printf("LSM6DSO [acc/mg]: %6d, %6d, %6d\r\n", axes[0], axes[1], axes[2]); - - acc_gyro->get_g_axes(axes); - printf("LSM6DSO [gyro/mdps]: %6d, %6d, %6d\r\n", axes[0], axes[1], axes[2]); - - ThisThread::sleep_for(1500); + normalizza(axes); + direzione = atan2(axes[1], axes[0]) * 180.0 / PI; + direzione+=180; + printf("gradi=%d\r\n",direzione); + ThisThread::sleep_for(500); } + + }