From caf7e76dad41fa44f77a67032a1d819cf3f877c8 Mon Sep 17 00:00:00 2001 From: Davide QUARANTIELLO Date: Fri, 29 Sep 2023 11:09:07 +0200 Subject: [PATCH] Add Reset_SFLP api, astyle on examples --- .../LSM6DSV16X_6D_Orientation.ino | 133 +++++++++--------- .../LSM6DSV16X_DataLog_Terminal.ino | 11 +- .../LSM6DSV16X_Double_Tap_Detection.ino | 14 +- .../LSM6DSV16X_FIFO_Interrupt.ino | 71 +++++----- .../LSM6DSV16X_FIFO_Polling.ino | 58 ++++---- .../LSM6DSV16X_Free_Fall_Detection.ino | 16 +-- .../LSM6DSV16X_Pedometer.ino | 19 ++- .../LSM6DSV16X_Qvar_Polling.ino | 22 +-- .../LSM6DSV16X_Sensor_Fusion.ino | 86 ++++++----- .../LSM6DSV16X_Single_Tap_Detection.ino | 16 +-- .../LSM6DSV16X_Tilt_Detection.ino | 16 +-- .../LSM6DSV16X_Wake_Up_Detection.ino | 16 +-- library.properties | 2 +- src/LSM6DSV16XSensor.cpp | 27 ++++ src/LSM6DSV16XSensor.h | 1 + 15 files changed, 277 insertions(+), 231 deletions(-) diff --git a/examples/LSM6DSV16X_6D_Orientation/LSM6DSV16X_6D_Orientation.ino b/examples/LSM6DSV16X_6D_Orientation/LSM6DSV16X_6D_Orientation.ino index e42b523..bbe2ddb 100644 --- a/examples/LSM6DSV16X_6D_Orientation/LSM6DSV16X_6D_Orientation.ino +++ b/examples/LSM6DSV16X_6D_Orientation/LSM6DSV16X_6D_Orientation.ino @@ -1,6 +1,6 @@ /* @file LSM6DSV16X_HelloWorld.ino - @author STMicroelectronics + @author STMicroelectronics @brief Example to use the LSM6DSV16X 6D Orientation ******************************************************************************* Copyright (c) 2022, STMicroelectronics @@ -24,7 +24,8 @@ char report[256]; void INT1Event_cb(); void sendOrientation(); -void setup() { +void setup() +{ // Initlialize serial. Serial.begin(115200); @@ -38,7 +39,7 @@ void setup() { // Enable INT1 pin. attachInterrupt(INT1_pin, INT1Event_cb, RISING); - + // Initlialize components. LSM6DSV16X.begin(); LSM6DSV16X.Enable_X(); @@ -47,18 +48,17 @@ void setup() { LSM6DSV16XStatusTypeDef ret = LSM6DSV16X.Enable_6D_Orientation(LSM6DSV16X_INT1_PIN); } -void loop() { - if (mems_event) - { +void loop() +{ + if (mems_event) { mems_event = 0; LSM6DSV16X_Event_Status_t status; LSM6DSV16X.Get_X_Event_Status(&status); - if (status.D6DOrientationStatus) - { + if (status.D6DOrientationStatus) { // Send 6D Orientation sendOrientation(); - + // Led blinking. digitalWrite(LED_BUILTIN, HIGH); delay(100); @@ -80,79 +80,72 @@ void sendOrientation() uint8_t yh = 0; uint8_t zl = 0; uint8_t zh = 0; - + LSM6DSV16X.Get_6D_Orientation_XL(&xl); LSM6DSV16X.Get_6D_Orientation_XH(&xh); LSM6DSV16X.Get_6D_Orientation_YL(&yl); LSM6DSV16X.Get_6D_Orientation_YH(&yh); LSM6DSV16X.Get_6D_Orientation_ZL(&zl); LSM6DSV16X.Get_6D_Orientation_ZH(&zh); - - if ( xl == 1 && yl == 0 && zl == 0 && xh == 0 && yh == 0 && zh == 0 ) - { - sprintf( report, "\r\n ________________ " \ - "\r\n | | " \ - "\r\n | * | " \ - "\r\n | | " \ - "\r\n | | " \ - "\r\n | | " \ - "\r\n | | " \ - "\r\n |________________| \r\n" ); + + if (xl == 1 && yl == 0 && zl == 0 && xh == 0 && yh == 0 && zh == 0) { + sprintf(report, "\r\n ________________ " \ + "\r\n | | " \ + "\r\n | * | " \ + "\r\n | | " \ + "\r\n | | " \ + "\r\n | | " \ + "\r\n | | " \ + "\r\n |________________| \r\n"); } - - else if ( xl == 0 && yl == 1 && zl == 0 && xh == 0 && yh == 0 && zh == 0 ) - { - sprintf( report, "\r\n ________________ " \ - "\r\n | | " \ - "\r\n | * | " \ - "\r\n | | " \ - "\r\n | | " \ - "\r\n | | " \ - "\r\n | | " \ - "\r\n |________________| \r\n" ); + + else if (xl == 0 && yl == 1 && zl == 0 && xh == 0 && yh == 0 && zh == 0) { + sprintf(report, "\r\n ________________ " \ + "\r\n | | " \ + "\r\n | * | " \ + "\r\n | | " \ + "\r\n | | " \ + "\r\n | | " \ + "\r\n | | " \ + "\r\n |________________| \r\n"); } - - else if ( xl == 0 && yl == 0 && zl == 0 && xh == 0 && yh == 1 && zh == 0 ) - { - sprintf( report, "\r\n ________________ " \ - "\r\n | | " \ - "\r\n | | " \ - "\r\n | | " \ - "\r\n | | " \ - "\r\n | | " \ - "\r\n | * | " \ - "\r\n |________________| \r\n" ); + + else if (xl == 0 && yl == 0 && zl == 0 && xh == 0 && yh == 1 && zh == 0) { + sprintf(report, "\r\n ________________ " \ + "\r\n | | " \ + "\r\n | | " \ + "\r\n | | " \ + "\r\n | | " \ + "\r\n | | " \ + "\r\n | * | " \ + "\r\n |________________| \r\n"); } - - else if ( xl == 0 && yl == 0 && zl == 0 && xh == 1 && yh == 0 && zh == 0 ) - { - sprintf( report, "\r\n ________________ " \ - "\r\n | | " \ - "\r\n | | " \ - "\r\n | | " \ - "\r\n | | " \ - "\r\n | | " \ - "\r\n | * | " \ - "\r\n |________________| \r\n" ); + + else if (xl == 0 && yl == 0 && zl == 0 && xh == 1 && yh == 0 && zh == 0) { + sprintf(report, "\r\n ________________ " \ + "\r\n | | " \ + "\r\n | | " \ + "\r\n | | " \ + "\r\n | | " \ + "\r\n | | " \ + "\r\n | * | " \ + "\r\n |________________| \r\n"); } - - else if ( xl == 0 && yl == 0 && zl == 0 && xh == 0 && yh == 0 && zh == 1 ) - { - sprintf( report, "\r\n __*_____________ " \ - "\r\n |________________| \r\n" ); + + else if (xl == 0 && yl == 0 && zl == 0 && xh == 0 && yh == 0 && zh == 1) { + sprintf(report, "\r\n __*_____________ " \ + "\r\n |________________| \r\n"); } - - else if ( xl == 0 && yl == 0 && zl == 1 && xh == 0 && yh == 0 && zh == 0 ) - { - sprintf( report, "\r\n ________________ " \ - "\r\n |________________| " \ - "\r\n * \r\n" ); + + else if (xl == 0 && yl == 0 && zl == 1 && xh == 0 && yh == 0 && zh == 0) { + sprintf(report, "\r\n ________________ " \ + "\r\n |________________| " \ + "\r\n * \r\n"); } - - else - { - sprintf( report, "None of the 6D orientation axes is set in accelrometer.\r\n" ); + + else { + sprintf(report, "None of the 6D orientation axes is set in accelrometer.\r\n"); } - + Serial.print(report); } diff --git a/examples/LSM6DSV16X_DataLog_Terminal/LSM6DSV16X_DataLog_Terminal.ino b/examples/LSM6DSV16X_DataLog_Terminal/LSM6DSV16X_DataLog_Terminal.ino index 288f05e..a6d1c13 100644 --- a/examples/LSM6DSV16X_DataLog_Terminal/LSM6DSV16X_DataLog_Terminal.ino +++ b/examples/LSM6DSV16X_DataLog_Terminal/LSM6DSV16X_DataLog_Terminal.ino @@ -14,10 +14,11 @@ #include -LSM6DSV16XSensor sensor (&Wire); +LSM6DSV16XSensor sensor(&Wire); int32_t accel[3], angrate[3]; -void setup() { +void setup() +{ pinMode(LED_BUILTIN, OUTPUT); Serial.begin(115200); Wire.begin(); @@ -26,7 +27,8 @@ void setup() { sensor.Enable_G(); } -void loop() { +void loop() +{ sensor.Get_X_Axes(accel); sensor.Get_G_Axes(angrate); @@ -47,7 +49,8 @@ void loop() { blink(LED_BUILTIN); } -inline void blink(int pin) { +inline void blink(int pin) +{ digitalWrite(pin, HIGH); delay(25); digitalWrite(pin, LOW); diff --git a/examples/LSM6DSV16X_Double_Tap_Detection/LSM6DSV16X_Double_Tap_Detection.ino b/examples/LSM6DSV16X_Double_Tap_Detection/LSM6DSV16X_Double_Tap_Detection.ino index 757ace4..fcadae1 100644 --- a/examples/LSM6DSV16X_Double_Tap_Detection/LSM6DSV16X_Double_Tap_Detection.ino +++ b/examples/LSM6DSV16X_Double_Tap_Detection/LSM6DSV16X_Double_Tap_Detection.ino @@ -1,6 +1,6 @@ /* @file LSM6DSV16X_Double_Tap_Detection.ino - @author STMicroelectronics + @author STMicroelectronics @brief Example to use the LSM6DSV16X Double Tap Detection ******************************************************************************* Copyright (c) 2022, STMicroelectronics @@ -21,7 +21,8 @@ LSM6DSV16XSensor LSM6DSV16X(&Wire); volatile int mems_event = 0; void INT1Event_cb(); -void setup() { +void setup() +{ // Initlialize serial. Serial.begin(115200); @@ -44,15 +45,14 @@ void setup() { LSM6DSV16X.Enable_Double_Tap_Detection(LSM6DSV16X_INT1_PIN); } -void loop() { - if (mems_event) - { +void loop() +{ + if (mems_event) { mems_event = 0; LSM6DSV16X_Event_Status_t status; LSM6DSV16X.Get_X_Event_Status(&status); - if (status.DoubleTapStatus) - { + if (status.DoubleTapStatus) { // Led blinking. digitalWrite(LED_BUILTIN, HIGH); delay(100); diff --git a/examples/LSM6DSV16X_FIFO_Interrupt/LSM6DSV16X_FIFO_Interrupt.ino b/examples/LSM6DSV16X_FIFO_Interrupt/LSM6DSV16X_FIFO_Interrupt.ino index cec3a41..8a30eda 100644 --- a/examples/LSM6DSV16X_FIFO_Interrupt/LSM6DSV16X_FIFO_Interrupt.ino +++ b/examples/LSM6DSV16X_FIFO_Interrupt/LSM6DSV16X_FIFO_Interrupt.ino @@ -36,25 +36,26 @@ uint32_t pos = 0; void Read_FIFO_Data(); -void setup() { - +void setup() +{ + Serial.begin(115200); Wire.begin(); // Enable INT1 pin. attachInterrupt(INT1_pin, INT1_fullEvent_cb, RISING); - + // Initialize LSM6DSV16X. AccGyr.begin(); status |= AccGyr.Enable_X(); status |= AccGyr.Enable_G(); - + // Configure ODR and FS of the acc and gyro status |= AccGyr.Set_X_ODR(SENSOR_ODR); status |= AccGyr.Set_X_FS(ACC_FS); status |= AccGyr.Set_G_ODR(SENSOR_ODR); status |= AccGyr.Set_G_FS(GYR_FS); - + // Configure FIFO BDR for acc and gyro status |= AccGyr.FIFO_Set_X_BDR(SENSOR_ODR); status |= AccGyr.FIFO_Set_G_BDR(SENSOR_ODR); @@ -62,37 +63,38 @@ void setup() { // Set Set FIFO watermark level status |= AccGyr.FIFO_Set_Watermark_Level(FIFO_SAMPLE_THRESHOLD); // Set FIFO stop on watermark level - status |= AccGyr.FIFO_Set_Stop_On_Fth(1); + status |= AccGyr.FIFO_Set_Stop_On_Fth(1); // Enable FIFO full interrupt on sensor INT1 pin - status |= AccGyr.FIFO_Set_INT1_FIFO_Full(1); + status |= AccGyr.FIFO_Set_INT1_FIFO_Full(1); // Set FIFO in Continuous mode status |= AccGyr.FIFO_Set_Mode(LSM6DSV16X_STREAM_MODE); - - if(status != LSM6DSV16X_OK) { + + if (status != LSM6DSV16X_OK) { Serial.println("LSM6DSV16X Sensor failed to init/configure"); - while(1); + while (1); } Serial.println("LSM6DSV16X FIFO Demo"); } -void loop() { +void loop() +{ uint8_t fullStatus = 0; - + // If we reach the threshold we can empty the FIFO if (fullFlag != 0) { fullFlag = 0; - - if(AccGyr.FIFO_Get_Full_Status(&fullStatus) != LSM6DSV16X_OK){ + + if (AccGyr.FIFO_Get_Full_Status(&fullStatus) != LSM6DSV16X_OK) { Serial.println("LSM6DSV16X Sensor failed to get full status"); - while(1); + while (1); } - if(fullStatus) { + if (fullStatus) { fullStatus = 0; - + // Empty the FIFO Read_FIFO_Data(); - + // Print FIFO data Serial.print(buff); } @@ -103,42 +105,42 @@ void Read_FIFO_Data() { uint16_t i; uint16_t samples_to_read; - + // Check the number of samples inside FIFO - if(AccGyr.FIFO_Get_Num_Samples(&samples_to_read) != LSM6DSV16X_OK){ - Serial.println("LSM6DSV16X Sensor failed to get number of samples inside FIFO"); - while(1); - } - + if (AccGyr.FIFO_Get_Num_Samples(&samples_to_read) != LSM6DSV16X_OK) { + Serial.println("LSM6DSV16X Sensor failed to get number of samples inside FIFO"); + while (1); + } + for (i = 0; i < samples_to_read; i++) { uint8_t tag; - + // Check the FIFO tag - if(AccGyr.FIFO_Get_Tag(&tag) != LSM6DSV16X_OK){ + if (AccGyr.FIFO_Get_Tag(&tag) != LSM6DSV16X_OK) { Serial.println("LSM6DSV16X Sensor failed to get tag"); - while(1); + while (1); } switch (tag) { // If we have a gyro tag, read the gyro data case 1: { - if(AccGyr.FIFO_Get_G_Axes(gyr_value) != LSM6DSV16X_OK){ + if (AccGyr.FIFO_Get_G_Axes(gyr_value) != LSM6DSV16X_OK) { Serial.println("LSM6DSV16X Sensor failed to get gyroscope data"); - while(1); + while (1); } gyr_available = true; break; } - + // If we have an acc tag, read the acc data case 2: { - if(AccGyr.FIFO_Get_X_Axes(acc_value) != LSM6DSV16X_OK){ + if (AccGyr.FIFO_Get_X_Axes(acc_value) != LSM6DSV16X_OK) { Serial.println("LSM6DSV16X Sensor failed to get accelerometer data"); - while(1); + while (1); } acc_available = true; break; } - + // We can discard other tags default: { break; @@ -160,6 +162,7 @@ void Read_FIFO_Data() } // ISR callback for INT1 -void INT1_fullEvent_cb() { +void INT1_fullEvent_cb() +{ fullFlag = 1; } diff --git a/examples/LSM6DSV16X_FIFO_Polling/LSM6DSV16X_FIFO_Polling.ino b/examples/LSM6DSV16X_FIFO_Polling/LSM6DSV16X_FIFO_Polling.ino index b2782fe..d3697d3 100644 --- a/examples/LSM6DSV16X_FIFO_Polling/LSM6DSV16X_FIFO_Polling.ino +++ b/examples/LSM6DSV16X_FIFO_Polling/LSM6DSV16X_FIFO_Polling.ino @@ -36,51 +36,53 @@ uint32_t pos = 0; void Read_FIFO_Data(); -void setup() { - +void setup() +{ + Serial.begin(115200); Wire.begin(); - + // Initialize LSM6DSV16X. AccGyr.begin(); status |= AccGyr.Enable_X(); status |= AccGyr.Enable_G(); - + // Configure ODR and FS of the acc and gyro status |= AccGyr.Set_X_ODR(SENSOR_ODR); status |= AccGyr.Set_X_FS(ACC_FS); status |= AccGyr.Set_G_ODR(SENSOR_ODR); status |= AccGyr.Set_G_FS(GYR_FS); - + // Configure FIFO BDR for acc and gyro status |= AccGyr.FIFO_Set_X_BDR(SENSOR_ODR); status |= AccGyr.FIFO_Set_G_BDR(SENSOR_ODR); // Set FIFO in Continuous mode status |= AccGyr.FIFO_Set_Mode(LSM6DSV16X_STREAM_MODE); - - if(status != LSM6DSV16X_OK) { + + if (status != LSM6DSV16X_OK) { Serial.println("LSM6DSV16X Sensor failed to init/configure"); - while(1); + while (1); } Serial.println("LSM6DSV16X FIFO Demo"); } -void loop() { +void loop() +{ uint16_t fifo_samples; // Check the number of samples inside FIFO - if(AccGyr.FIFO_Get_Num_Samples(&fifo_samples) != LSM6DSV16X_OK){ - Serial.println("LSM6DSV16X Sensor failed to get number of samples inside FIFO"); - while(1); + if (AccGyr.FIFO_Get_Num_Samples(&fifo_samples) != LSM6DSV16X_OK) { + Serial.println("LSM6DSV16X Sensor failed to get number of samples inside FIFO"); + while (1); } // If we reach the threshold we can empty the FIFO if (fifo_samples > FIFO_SAMPLE_THRESHOLD) { - + // Empty the FIFO Read_FIFO_Data(); - + // Print FIFO data Serial.print(buff); } @@ -90,42 +92,42 @@ void Read_FIFO_Data() { uint16_t i; uint16_t samples_to_read; - + // Check the number of samples inside FIFO - if(AccGyr.FIFO_Get_Num_Samples(&samples_to_read) != LSM6DSV16X_OK){ - Serial.println("LSM6DSV16X Sensor failed to get number of samples inside FIFO"); - while(1); + if (AccGyr.FIFO_Get_Num_Samples(&samples_to_read) != LSM6DSV16X_OK) { + Serial.println("LSM6DSV16X Sensor failed to get number of samples inside FIFO"); + while (1); } - + for (i = 0; i < samples_to_read; i++) { uint8_t tag; - + // Check the FIFO tag - if(AccGyr.FIFO_Get_Tag(&tag) != LSM6DSV16X_OK){ + if (AccGyr.FIFO_Get_Tag(&tag) != LSM6DSV16X_OK) { Serial.println("LSM6DSV16X Sensor failed to get tag"); - while(1); + while (1); } switch (tag) { // If we have a gyro tag, read the gyro data case 1: { - if(AccGyr.FIFO_Get_G_Axes(gyr_value) != LSM6DSV16X_OK){ + if (AccGyr.FIFO_Get_G_Axes(gyr_value) != LSM6DSV16X_OK) { Serial.println("LSM6DSV16X Sensor failed to get gyroscope data"); - while(1); + while (1); } gyr_available = true; break; } - + // If we have an acc tag, read the acc data case 2: { - if(AccGyr.FIFO_Get_X_Axes(acc_value) != LSM6DSV16X_OK){ + if (AccGyr.FIFO_Get_X_Axes(acc_value) != LSM6DSV16X_OK) { Serial.println("LSM6DSV16X Sensor failed to get accelerometer data"); - while(1); + while (1); } acc_available = true; break; } - + // We can discard other tags default: { break; diff --git a/examples/LSM6DSV16X_Free_Fall_Detection/LSM6DSV16X_Free_Fall_Detection.ino b/examples/LSM6DSV16X_Free_Fall_Detection/LSM6DSV16X_Free_Fall_Detection.ino index a72e95e..7beae01 100644 --- a/examples/LSM6DSV16X_Free_Fall_Detection/LSM6DSV16X_Free_Fall_Detection.ino +++ b/examples/LSM6DSV16X_Free_Fall_Detection/LSM6DSV16X_Free_Fall_Detection.ino @@ -1,6 +1,6 @@ /* @file LSM6DSV16X_Free_Fall_Detection.ino - @author STMicroelectronics + @author STMicroelectronics @brief Example to use the LSM6DSV16X Free Fall Detection ******************************************************************************* Copyright (c) 2022, STMicroelectronics @@ -22,7 +22,8 @@ volatile int mems_event = 0; void INT1Event_cb(); -void setup() { +void setup() +{ // Initlialize serial. Serial.begin(115200); @@ -36,7 +37,7 @@ void setup() { // Enable INT1 pin. attachInterrupt(INT1_pin, INT1Event_cb, RISING); - + // Initlialize components. LSM6DSV16X.begin(); LSM6DSV16X.Enable_X(); @@ -45,15 +46,14 @@ void setup() { LSM6DSV16X.Enable_Free_Fall_Detection(LSM6DSV16X_INT1_PIN); } -void loop() { - if (mems_event) - { +void loop() +{ + if (mems_event) { mems_event = 0; LSM6DSV16X_Event_Status_t status; LSM6DSV16X.Get_X_Event_Status(&status); - if (status.FreeFallStatus) - { + if (status.FreeFallStatus) { // Led blinking. digitalWrite(LED_BUILTIN, HIGH); delay(100); diff --git a/examples/LSM6DSV16X_Pedometer/LSM6DSV16X_Pedometer.ino b/examples/LSM6DSV16X_Pedometer/LSM6DSV16X_Pedometer.ino index 8410f12..b2a4f52 100644 --- a/examples/LSM6DSV16X_Pedometer/LSM6DSV16X_Pedometer.ino +++ b/examples/LSM6DSV16X_Pedometer/LSM6DSV16X_Pedometer.ino @@ -1,6 +1,6 @@ /* @file LSM6DSV16X_Pedometer.ino - @author STMicroelectronics + @author STMicroelectronics @brief Example to use the LSM6DSV16X Pedometer ******************************************************************************* Copyright (c) 2022, STMicroelectronics @@ -26,7 +26,8 @@ char report[256]; void INT1Event_cb(); -void setup() { +void setup() +{ // Initlialize serial. Serial.begin(115200); @@ -40,7 +41,7 @@ void setup() { // Enable INT1 pin. attachInterrupt(INT1_pin, INT1Event_cb, RISING); - + // Initlialize components. LSM6DSV16X.begin(); LSM6DSV16X.Enable_X(); @@ -51,15 +52,14 @@ void setup() { previous_tick = millis(); } -void loop() { - if (mems_event) - { +void loop() +{ + if (mems_event) { mems_event = 0; LSM6DSV16X_Event_Status_t status; LSM6DSV16X.Get_X_Event_Status(&status); - if (status.StepStatus) - { + if (status.StepStatus) { // Led blinking. digitalWrite(LED_BUILTIN, HIGH); delay(100); @@ -72,8 +72,7 @@ void loop() { } // Print the step counter in any case every 3000 ms uint32_t current_tick = millis(); - if((current_tick - previous_tick) >= 3000) - { + if ((current_tick - previous_tick) >= 3000) { LSM6DSV16X.Get_Step_Count(&step_count); snprintf(report, sizeof(report), "Step counter: %d", step_count); Serial.println(report); diff --git a/examples/LSM6DSV16X_Qvar_Polling/LSM6DSV16X_Qvar_Polling.ino b/examples/LSM6DSV16X_Qvar_Polling/LSM6DSV16X_Qvar_Polling.ino index 4eaab10..a159242 100644 --- a/examples/LSM6DSV16X_Qvar_Polling/LSM6DSV16X_Qvar_Polling.ino +++ b/examples/LSM6DSV16X_Qvar_Polling/LSM6DSV16X_Qvar_Polling.ino @@ -16,35 +16,35 @@ #include LSM6DSV16XSensor AccGyr(&Wire); -void setup() { - +void setup() +{ + Serial.begin(115200); Wire.begin(); - + // Initialize LSM6DSV16X. AccGyr.begin(); - + // Enable accelerometer and gyroscope. AccGyr.Enable_X(); AccGyr.Enable_G(); // Enable QVAR - if(AccGyr.QVAR_Enable()!= 0) - { + if (AccGyr.QVAR_Enable() != 0) { Serial.println("Error during initialization of QVAR"); - while(1); + while (1); } Serial.println("LSM6DSV16X QVAR Demo"); } -void loop() { +void loop() +{ uint8_t qvar_status; float qvar_data; // Check if QVAR data is ready AccGyr.QVAR_GetStatus(&qvar_status); - if(qvar_status) - { - // Get QVAR data + if (qvar_status) { + // Get QVAR data AccGyr.QVAR_GetData(&qvar_data); Serial.println(qvar_data); } diff --git a/examples/LSM6DSV16X_Sensor_Fusion/LSM6DSV16X_Sensor_Fusion.ino b/examples/LSM6DSV16X_Sensor_Fusion/LSM6DSV16X_Sensor_Fusion.ino index 6002c43..79764e8 100644 --- a/examples/LSM6DSV16X_Sensor_Fusion/LSM6DSV16X_Sensor_Fusion.ino +++ b/examples/LSM6DSV16X_Sensor_Fusion/LSM6DSV16X_Sensor_Fusion.ino @@ -17,17 +17,22 @@ #define ALGO_FREQ 120U /* Algorithm frequency 120Hz */ #define ALGO_PERIOD (1000U / ALGO_FREQ) /* Algorithm period [ms] */ -unsigned long startTime, elapsedTime; - +unsigned long startTime, elapsedTime; + LSM6DSV16XSensor AccGyr(&Wire); uint8_t status = 0; +uint32_t k = 0; + uint8_t tag = 0; -float quaternions[4]={0}; +float quaternions[4] = {0}; + +void setup() +{ -void setup() { - Serial.begin(115200); - while (!Serial) yield(); + while (!Serial) { + yield(); + } Wire.begin(); // Initialize LSM6DSV16X. @@ -36,44 +41,57 @@ void setup() { // Enable Sensor Fusion status |= AccGyr.Enable_Rotation_Vector(); - if(status != LSM6DSV16X_OK) { + if (status != LSM6DSV16X_OK) { Serial.println("LSM6DSV16X Sensor failed to init/configure"); - while(1); + while (1); } Serial.println("LSM6DSV16X SFLP Demo"); } -void loop() { +void loop() +{ uint16_t fifo_samples; // Get start time of loop cycle - startTime = millis(); + startTime = millis(); + // Check the number of samples inside FIFO - if(AccGyr.FIFO_Get_Num_Samples(&fifo_samples) != LSM6DSV16X_OK){ - Serial.println("LSM6DSV16X Sensor failed to get number of samples inside FIFO"); - while(1); + if (AccGyr.FIFO_Get_Num_Samples(&fifo_samples) != LSM6DSV16X_OK) { + Serial.println("LSM6DSV16X Sensor failed to get number of samples inside FIFO"); + while (1); } - - // Read the FIFO if there is one stored sample + + // Read the FIFO if there is one stored sample if (fifo_samples > 0) { - for (int i = 0; i < fifo_samples; i++) { - AccGyr.FIFO_Get_Tag(&tag); - if(tag==0x13u){ - AccGyr.FIFO_Get_Rotation_Vector(&quaternions[0]); - - // Print Quaternion data - Serial.print("Quaternion: "); - Serial.print(quaternions[3], 4); - Serial.print(", "); - Serial.print(-quaternions[1], 4); - Serial.print(", "); - Serial.print(quaternions[0], 4); - Serial.print(", "); - Serial.println(quaternions[2], 4); - - // Compute the elapsed time within loop cycle and wait - elapsedTime = millis() - startTime; - delay(ALGO_PERIOD - elapsedTime); - } + for (int i = 0; i < fifo_samples; i++) { + AccGyr.FIFO_Get_Tag(&tag); + if (tag == 0x13u) { + AccGyr.FIFO_Get_Rotation_Vector(&quaternions[0]); + + // Print Quaternion data + Serial.print("Quaternion: "); + Serial.print(quaternions[3], 4); + Serial.print(", "); + Serial.print(-quaternions[1], 4); + Serial.print(", "); + Serial.print(quaternions[0], 4); + Serial.print(", "); + Serial.println(quaternions[2], 4); + + // Compute the elapsed time within loop cycle and wait + elapsedTime = millis() - startTime; + + if ((long)(ALGO_PERIOD - elapsedTime)) { + delay(ALGO_PERIOD - elapsedTime); + } } + } } } + + + + + + + + diff --git a/examples/LSM6DSV16X_Single_Tap_Detection/LSM6DSV16X_Single_Tap_Detection.ino b/examples/LSM6DSV16X_Single_Tap_Detection/LSM6DSV16X_Single_Tap_Detection.ino index 6595863..2039f25 100644 --- a/examples/LSM6DSV16X_Single_Tap_Detection/LSM6DSV16X_Single_Tap_Detection.ino +++ b/examples/LSM6DSV16X_Single_Tap_Detection/LSM6DSV16X_Single_Tap_Detection.ino @@ -1,6 +1,6 @@ /* @file LSM6DSV16X_Single_Tap_Detection.ino - @author STMicroelectronics + @author STMicroelectronics @brief Example to use the LSM6DSV16X Single Tap Detection ******************************************************************************* Copyright (c) 2022, STMicroelectronics @@ -21,7 +21,8 @@ volatile int mems_event = 0; void INT1Event_cb(); -void setup() { +void setup() +{ Serial.begin(115200); delay(1000); pinMode(LED_BUILTIN, OUTPUT); @@ -29,7 +30,7 @@ void setup() { //Interrupts. attachInterrupt(INT1_pin, INT1Event_cb, RISING); - + // Initlialize components. LSM6DSV16X.begin(); LSM6DSV16X.Enable_X(); @@ -38,14 +39,13 @@ void setup() { LSM6DSV16X.Enable_Single_Tap_Detection(LSM6DSV16X_INT1_PIN); } -void loop() { - if (mems_event) - { +void loop() +{ + if (mems_event) { mems_event = 0; LSM6DSV16X_Event_Status_t status; LSM6DSV16X.Get_X_Event_Status(&status); - if (status.TapStatus) - { + if (status.TapStatus) { // Led blinking. digitalWrite(LED_BUILTIN, HIGH); diff --git a/examples/LSM6DSV16X_Tilt_Detection/LSM6DSV16X_Tilt_Detection.ino b/examples/LSM6DSV16X_Tilt_Detection/LSM6DSV16X_Tilt_Detection.ino index b7036b3..9bbdd6b 100644 --- a/examples/LSM6DSV16X_Tilt_Detection/LSM6DSV16X_Tilt_Detection.ino +++ b/examples/LSM6DSV16X_Tilt_Detection/LSM6DSV16X_Tilt_Detection.ino @@ -1,6 +1,6 @@ /* @file LSM6DSV16X_Tilt_Detection.ino - @author STMicroelectronics + @author STMicroelectronics @brief Example to use the LSM6DSV16X Tilt Detection ******************************************************************************* Copyright (c) 2022, STMicroelectronics @@ -22,7 +22,8 @@ volatile int mems_event = 0; void INT1Event_cb(); -void setup() { +void setup() +{ // Initlialize serial. Serial.begin(115200); @@ -36,7 +37,7 @@ void setup() { // Enable INT1 pin. attachInterrupt(INT1_pin, INT1Event_cb, RISING); - + // Initlialize components. LSM6DSV16X.begin(); LSM6DSV16X.Enable_X(); @@ -45,15 +46,14 @@ void setup() { LSM6DSV16X.Enable_Tilt_Detection(LSM6DSV16X_INT1_PIN); } -void loop() { - if (mems_event) - { +void loop() +{ + if (mems_event) { mems_event = 0; LSM6DSV16X_Event_Status_t status; LSM6DSV16X.Get_X_Event_Status(&status); - if (status.TiltStatus) - { + if (status.TiltStatus) { // Led blinking. digitalWrite(LED_BUILTIN, HIGH); delay(100); diff --git a/examples/LSM6DSV16X_Wake_Up_Detection/LSM6DSV16X_Wake_Up_Detection.ino b/examples/LSM6DSV16X_Wake_Up_Detection/LSM6DSV16X_Wake_Up_Detection.ino index c5a0395..23ea7c9 100644 --- a/examples/LSM6DSV16X_Wake_Up_Detection/LSM6DSV16X_Wake_Up_Detection.ino +++ b/examples/LSM6DSV16X_Wake_Up_Detection/LSM6DSV16X_Wake_Up_Detection.ino @@ -24,7 +24,8 @@ volatile int mems_event = 0; void INT1Event_cb(); -void setup() { +void setup() +{ // Initlialize serial. Serial.begin(115200); @@ -38,7 +39,7 @@ void setup() { // Enable INT1 pin. attachInterrupt(INT1_pin, INT1Event_cb, RISING); - + // Initlialize components. LSM6DSV16X.begin(); LSM6DSV16X.Enable_X(); @@ -47,19 +48,18 @@ void setup() { LSM6DSV16X.Enable_Wake_Up_Detection(LSM6DSV16X_INT1_PIN); } -void loop() { - if (mems_event) - { +void loop() +{ + if (mems_event) { mems_event = 0; LSM6DSV16X_Event_Status_t status; LSM6DSV16X.Get_X_Event_Status(&status); - if (status.WakeUpStatus) - { + if (status.WakeUpStatus) { // Led blinking. digitalWrite(LED_BUILTIN, HIGH); delay(100); digitalWrite(LED_BUILTIN, LOW); - + Serial.println("Wake up Detected!"); } } diff --git a/library.properties b/library.properties index de0c670..28c09d2 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=STM32duino LSM6DSV16X -version=1.6.3 +version=1.7.0 author=SRA maintainer=stm32duino sentence=Ultra Low Power inertial measurement unit. diff --git a/src/LSM6DSV16XSensor.cpp b/src/LSM6DSV16XSensor.cpp index 018df2f..3e47eca 100644 --- a/src/LSM6DSV16XSensor.cpp +++ b/src/LSM6DSV16XSensor.cpp @@ -3547,6 +3547,33 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Get_Gyroscope_Bias(float *gbias) return LSM6DSV16X_OK; } +/** + * @brief Reset SFLP + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Reset_SFLP(void) +{ + lsm6dsv16x_emb_func_init_a_t emb_func_init_a; + + if (lsm6dsv16x_mem_bank_set(®_ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_EMB_FUNC_INIT_A, (uint8_t *)&emb_func_init_a, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + emb_func_init_a.sflp_game_init = 1; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_EMB_FUNC_INIT_A, (uint8_t *)&emb_func_init_a, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_mem_bank_set(®_ctx, LSM6DSV16X_MAIN_MEM_BANK) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + return LSM6DSV16X_OK; +} /** * @brief Get the LSM6DSV16X register value diff --git a/src/LSM6DSV16XSensor.h b/src/LSM6DSV16XSensor.h index f34a97c..fd27e67 100644 --- a/src/LSM6DSV16XSensor.h +++ b/src/LSM6DSV16XSensor.h @@ -232,6 +232,7 @@ class LSM6DSV16XSensor { LSM6DSV16XStatusTypeDef FIFO_Get_Rotation_Vector(float *rvec); LSM6DSV16XStatusTypeDef FIFO_Get_Gravity_Vector(float *gvec); LSM6DSV16XStatusTypeDef FIFO_Get_Gyroscope_Bias(float *gbias); + LSM6DSV16XStatusTypeDef Reset_SFLP(); LSM6DSV16XStatusTypeDef Read_Reg(uint8_t Reg, uint8_t *Data); LSM6DSV16XStatusTypeDef Write_Reg(uint8_t Reg, uint8_t Data);