search

STMicroelectronics / lis2dw12-pid

lis2dw12 platform independent driver based on Standard C language and compliant with MISRA standard
BSD 3-Clause "New" or "Revised" License
13 stars 4 forks source link

Incorrect orientation with LIS2DW12TR #2

Closed ybastiand closed 4 months ago

ybastiand commented 5 months ago

Hello,

In our product we are using the accelerometer LIS2DW12TR. We have issue whereby we sometimes get orientation different from ACCEL_ORIENTATION_UPRIGHT in our driver, even though the accelerometer is upright. It may have moved (probably has moved actually), but definitely stayed upright.

I looked at the file STMems_Standard_C_drivers/lis2dw12_STdC/examples/lis2dw12_orientation.c at master · STMicroelectronics/STMems_Standard_C_drivers · GitHub, and noticed that there is the following line: lis2dw12_6d_feed_data_set(&dev_ctx, LIS2DW12_LPF2_FEED); This seems to set the bit LPASS_ON6D in register CTRL7. In our driver we have that bit cleared.

Do you think setting that bit might fix our issue?

Another thought I have, is that whenever we get any kind of interrupt from the LIS2DW12TR, we read the orientation register SIXD_SRC, and update our accel_app->data.orientation. Could it be that the data in that register is sometimes incorrect, and maybe the other bits in that register are correct only if the 6D_IA bit is set? But if so, how can we know what the orientation is at boot-up of our device?

I’ve attached our driver in case you’d like to take a look. The following function converts the SIXD_SRC register to orientation.

//! Analyzes the 6id byte to obtain orientation void AccelAnalyzeAllSources(accel_app_st_t *accel_app) { lis2dw12_sixd_src_t sixd_src = accel_app->all_sources.sixd_src;

if (1 == sixd_src.zh) {
    accel_app->data.orientation = ACCEL_ORIENTATION_UPRIGHT;
} 
else if (1 == sixd_src.zl) {
    accel_app->data.orientation = ACCEL_ORIENTATION_UPSIDE_DOWN;
} 
else if (1 == sixd_src.yh) {
    accel_app->data.orientation = ACCEL_ORIENTATION_ON_RIGHTSIDE;
} 
else if (1 == sixd_src.yl) {
    accel_app->data.orientation = ACCEL_ORIENTATION_ON_LEFTSIDE;
} 
else if (1 == sixd_src.xh) {
    accel_app->data.orientation = ACCEL_ORIENTATION_NOSE_DOWN;
} 
else if (1 == sixd_src.xl) {
    accel_app->data.orientation = ACCEL_ORIENTATION_NOSE_UP;
} 
else {
    accel_app->data.orientation = ACCEL_ORIENTATION_UNKNOWN;
}

Thanks,

Yves Bastiand

yves.bastiand@siemens-healthineers.com Senior Embedded Software Engineer Systems Engineering Epocal, a Siemens Healthineers company 855 Brookfield Road, Suite 101 Ottawa, Ontario, K1V 2S5, Canada

Our driver source code: ++++++++++++++++++++++++++++++++++++++Lis2dw12AccelDriver.h: //! \file //! //! \copyright Copyright (c) 2017 by Epocal. All rights reserved. //! //! THIS SOURCE CODE CONTAINS CONFIDENTIAL INFORMATION THAT IS OWNED BY EPOCAL, //! AND MAY NOT BE COPIED, DISCLOSED OR OTHERWISE USED WITHOUT //! THE EXPRESS WRITTEN CONSENT OF EPOCAL. //! //! \brief Accelerometer Driver

// Header guard ------------------------------------------------------------------------------------

ifndef LIS2DW12ACCELDRIVER_H

define LIS2DW12ACCELDRIVER_H

// Includes ----------------------------------------------------------------------------------------

include "AccelModel.h"

include "I2cArbiterCmn.h"

include "SehI2cCmn.h"

// Header guard for accel driver type --------------------------------------------------------------

ifdef ACCEL_LIS2DW12

// Constants ---------------------------------------------------------------------------------------

define ACCEL_I2C_DEVICE_ADDRESS 0x33U

define ACCEL_SLEEPSTATE_CONFIG_ON 0x42U

define ACCEL_SLEEPSTATE_CONFIG_OFF 0x02U

// When accelerometer at rest, acceleration is 9.81 m/s/s for the vertical // dimension and 0 in other two dimensions

define ACCEL_NO_ACCELERATION 9.81f

constexpr bool HIGH_PERFORMANCE_MODE = true; //constexpr bool LOW_POWER_MODE = false;

// Typedefs ----------------------------------------------------------------------------------------

typedef enum { ACCEL_STATE_UNDEFINED, ACCEL_STATE_PWR_ON, ACCEL_STATE_CFG_WRITE, ACCEL_STATE_CFG_READ, ACCEL_STATE_READY, ACCEL_STATE_READ_INTERRUPT, ACCEL_STATE_READ_ORIENTATION, ACCEL_STATE_ANALYZE_ORIENTATION, ACCEL_STATE_READ_FF, ACCEL_STATE_READ_XYZ, ACCEL_STATE_ANALYZE_XYZ, } accel_state_en_t;

typedef enum { ACCEL_ERR_NONE, ACCEL_ERR_FAILED_TO_BOOT, ACCEL_ERR_CONFIG_WRITE, ACCEL_ERR_INTERRUPT_READ, ACCEL_ERR_ORIENT_READ, ACCEL_ERR_FF_READ, ACCEL_ERR_XYZ_READ, ACCEL_ERR_ARB_QUEUE_FULL } accel_error_en_t;

typedef enum { ACCEL_FULL_SCALE_2G = 0x00, ACCEL_FULL_SCALE_4G = 0x10, ACCEL_FULL_SCALE_8G = 0x20, ACCEL_FULL_SCALE_16G = 0x30 } accel_full_scale_en_t;

typedef enum { ACCEL_RESOLUTION_LOW_POWER_1 = 0x00, ACCEL_RESOLUTION_LOW_POWER_2 = 0x01, ACCEL_RESOLUTION_LOW_POWER_3 = 0x02, ACCEL_RESOLUTION_LOW_POWER_4 = 0x03, ACCEL_RESOLUTION_HIGH = 0x04, ACCEL_SINGLE_DATA_CONVERSION_ON_DEMAND_1 = 0x08, ACCEL_SINGLE_DATA_CONVERSION_ON_DEMAND_2 = 0x09, ACCEL_SINGLE_DATA_CONVERSION_ON_DEMAND_3 = 0x0A, ACCEL_SINGLE_DATA_CONVERSION_ON_DEMAND_4 = 0x0B } accel_resolution_en_t;

typedef struct { uint8_t drdy : 1; uint8_t ff_ia : 1; uint8_t _6d_ia : 1; uint8_t single_tap : 1; uint8_t double_tap : 1; uint8_t sleep_state_ia : 1; uint8_t drdy_t : 1; uint8_t ovr : 1; } lis2dw12_status_dup_t;

typedef struct { uint8_t z_wu : 1; uint8_t y_wu : 1; uint8_t x_wu : 1; uint8_t wu_ia : 1; uint8_t sleep_state_ia : 1; uint8_t ff_ia : 1; uint8_t not_used_01 : 2; } lis2dw12_wake_up_src_t;

typedef struct { uint8_t z_tap : 1; uint8_t y_tap : 1; uint8_t x_tap : 1; uint8_t tap_sign : 1; uint8_t double_tap : 1; uint8_t single_tap : 1; uint8_t tap_ia : 1; uint8_t not_used_01 : 1; } lis2dw12_tap_src_t;

typedef struct { uint8_t xl : 1; uint8_t xh : 1; uint8_t yl : 1; uint8_t yh : 1; uint8_t zl : 1; uint8_t zh : 1; uint8_t _6d_ia : 1; uint8_t not_used_01 : 1; } lis2dw12_sixd_src_t;

typedef struct { uint8_t ff_ia : 1; uint8_t wu_ia : 1; uint8_t single_tap : 1; uint8_t double_tap : 1; uint8_t _6d_ia : 1; uint8_t sleep_change_ia : 1; uint8_t not_used_01 : 2; } lis2dw12_all_int_src_t;

typedef struct { lis2dw12_status_dup_t status_dup; lis2dw12_wake_up_src_t wake_up_src; lis2dw12_tap_src_t tap_src; lis2dw12_sixd_src_t sixd_src; lis2dw12_all_int_src_t all_int_src; } lis2dw12_all_sources_t;

static_assert(5 == sizeof(lis2dw12_all_sources_t), "lis2dw12_all_sources changed size or there is padding");

typedef struct { accel_state_en_t state; // This is added to the state, because the config is written to and read back, both at boot-up // and when changing the sleep state bool is_booting_up; accel_error_en_t error; accel_interrupt_en_t interrupt_type; accel_data_st_t data; accel_data_st_t prev_data_buffer; uint16_t device_addr;

struct device_app_st    *device_app;
i2c_arb_app_st_t        *i2c_arb_app;
i2c_arb_req_st_t        request;

lis2dw12_all_sources_t  all_sources;

int                     write_config_step;
int                     rx_time_0;
uint8_t                 raw_pl_status;
uint8_t                 raw_ff_mt_src;
uint8_t                 raw_xyz_array[2 * ACCEL_NUM_COOR]; // 2 registers per coordinate

} accel_app_st_t;

// Functions ---------------------------------------------------------------------------------------

bool AccelIsBootupComplete(accel_app_st_t accel_app); void AccelExtiIsr(accel_app_st_t accel_app, accel_interrupt_en_t interr_type); float AccelResolutionToMg(accel_app_st_t accel_app, int16_t lsb, accel_full_scale_en_t full_scale, bool high_performance_mode,accel_resolution_en_t res); float AccelCalculateMagnitude(const accel_data_st_t accel_data); void AccelStartBoot(accel_app_st_t accel_app); void AccelWriteConfig(accel_app_st_t accel_app); void AccelReadConfig(accel_app_st_t accel_app); void AccelReadAllSources(accel_app_st_t accel_app); void AccelAnalyzeAllSources(accel_app_st_t accel_app); void AccelReadXYZ(accel_app_st_t accel_app); void AccelAnalyzeXYZ(accel_app_st_t accel_app); void AccelEventHandler(accel_app_st_t accel_app); void AccelChangeSleepstate(accel_app_st_t accel_app, uint8_t sleepstate_config); void AccelerometerSelfCheck(struct device_app_st device_app); void AccelInit(accel_app_st_t accel_app, i2c_arb_app_st_t arb, struct device_app_st *device_app);

endif

endif // ACCELDRIVER_H

++++++++++++++++++++++++++++++++++++++Lis2dw12AccelDriver.cpp: //! \file //! \author Eric Huang //! \date May-2022 //! \copyright Copyright (c) 2022 by Epocal. All rights reserved. //! //! THIS SOURCE CODE CONTAINS CONFIDENTIAL INFORMATION THAT IS OWNED BY EPOCAL, //! AND MAY NOT BE COPIED, DISCLOSED OR OTHERWISE USED WITHOUT //! THE EXPRESS WRITTEN CONSENT OF EPOCAL. //! //! \brief Accelerometer Driver

// Includes ----------------------------------------------------------------------------------------

include "i2c.h"

include "DeviceModelCmn.h"

include "DeviceSelfTestController.h"

include "DeviceTestController.h"

include "Lis2dw12AccelDriver.h"

include "I2cArbiterCmn.h"

include "InterfaceToHost.h"

include "Ng.Ntf.Accel.h"

include "SwCommunicationEvents.h"

include "ExternalInterrupt.h"

include "DevicePeripheralMonitorController.h"

// Accel Header Guard ------------------------------------------------------------------------------

ifdef ACCEL_LIS2DW12

// Constants ---------------------------------------------------------------------------------------

define ACCEL_GRAVITY 0.00981 // ms^2 per mg unit

define ACCEL_MAX_READ_TIME 100

define ACCEL_NUM_CONFIG_WRITES 5

define ACCEL_ON_TIMEOUT 1

define ACCEL_ON_TRIALS 10

define ACCEL_REG_SIZE 1

define ACCEL_UINT8_T_SIZE 1

define ACCEL_XYZ_ADR 0x01

define ACCEL_NUM_MAX_I2C_XFER_ATTEMPTS 3

// register locations

define LIS2DW12_OUT_T_L 0x0DU

define LIS2DW12_OUT_T_H 0x0EU

define LIS2DW12_WHO_AM_I 0x0FU

define LIS2DW12_CTRL1 0x20U

define LIS2DW12_CTRL2 0x21U

define LIS2DW12_CTRL3 0x22U

define LIS2DW12_CTRL4_INT1_PAD_CTRL 0x23U

define LIS2DW12_CTRL5_INT2_PAD_CTRL 0x24U

define LIS2DW12_CTRL6 0x25U

define LIS2DW12_OUT_T 0x26U

define LIS2DW12_STATUS 0x27U

define LIS2DW12_OUT_X_L 0x28U

define LIS2DW12_OUT_X_H 0x29U

define LIS2DW12_OUT_Y_L 0x2AU

define LIS2DW12_OUT_Y_H 0x2BU

define LIS2DW12_OUT_Z_L 0x2CU

define LIS2DW12_OUT_Z_H 0x2DU

define LIS2DW12_FIFO_CTRL 0x2EU

define LIS2DW12_FIFO_SAMPLES 0x2FU

define LIS2DW12_TAP_THS_X 0x30U

define LIS2DW12_TAP_THS_Y 0x31U

define LIS2DW12_TAP_THS_Z 0x32U

define LIS2DW12_INT_DUR 0x33U

define LIS2DW12_WAKE_UP_THS 0x34U

define LIS2DW12_WAKE_UP_DUR 0x35U

define LIS2DW12_FREE_FALL 0x36U

define LIS2DW12_STATUS_DUP 0x37U

define LIS2DW12_WAKE_UP_SRC 0x38U

define LIS2DW12_TAP_SRC 0x39U

define LIS2DW12_SIXD_SRC 0x3AU

define LIS2DW12_ALL_INT_SRC 0x3BU

define LIS2DW12_X_OFS_USR 0x3CU

define LIS2DW12_Y_OFS_USR 0x3DU

define LIS2DW12_Z_OFS_USR 0x3EU

define LIS2DW12_CTRL_REG7 0x3FU

// Globals -----------------------------------------------------------------------------------------

// singleton inline functions to help choose configurations inline uint8_t ReturnFullScaleConfig(uint8_t prev_config, accel_full_scale_en_t full_scale) { return prev_config & 0xCF | (uint8_t)full_scale; }

inline uint8_t ReturnResolutionConfig(uint8_t prev_config, accel_resolution_en_t res, accel_resolution_en_t sleep_power_mode) { return prev_config & 0xF0 | (uint8_t)res | (uint8_t)sleep_power_mode; }

// Command arrays to send during config uint8_t accel_config_cmd_0[] = {
// 0x20: CTRL1 ODR = 400Hz, high performance (necessary for double tap) // Selected low power mode when accelerometer sleep mode enable // LP mode 2 saves the most power with a 14 bit resolution ReturnResolutionConfig(0x74, ACCEL_RESOLUTION_HIGH, ACCEL_RESOLUTION_LOW_POWER_2), };

uint8_t accel_config_cmd_1[] = { 0xF8, // 0x23: CTRL4_INT1_PAD_CTRL, allow all interrupts except FIFO + data 0x40, // 0x24: CTRL5_INT2_PAD_CTRL, Enable sleep state change interrupt };

uint8_t accel_config_cmd_2[] = {

ifdef ACCEL_TIMING

// for testing the freefall timing, it helps to have a higher tap threshold
// (or else picking it up/dropping it triggers a wake up and tap before freefall)
0x45,               // 0x30: TAP_THS_X, set 6D threshold to 60, and x threshold
0xE5,               // 0x31: defaults for these registers
0xE5,               // 0x32: TAP_THS_Z, enable all taps

else

0x41,               // 0x30: TAP_THS_X, set 6D threshold to 60, and x threshold
0xE1,               // 0x31: defaults for these registers
0xE1,               // 0x32: TAP_THS_Z, enable all taps

endif

0x7F,               // 0x33: INT_DUR, set to double tap
ACCEL_SLEEPSTATE_CONFIG_ON, // 0x34: Wake up threshold, enable single tap and sleep
0x42,               // 0x35: Wake up duration, set it a bit longer to differentiate from taps
0xF8,               // 0x36: Free_fall detection, duration 78mS, threshold 156 mG

};

uint8_t accel_sleepstate;

uint8_t accel_config_cmd_3[] = { 0x20 // 0x3f CTRL7: enable interrupts };

uint8_t accel_config_cmd_4[] = { // 0x25, CTRL6, at 2g selection ReturnFullScaleConfig(0x04, ACCEL_FULL_SCALE_2G) };

// Addresses of target registers for config uint8_t accel_config_addr[ACCEL_NUM_CONFIG_WRITES] = { LIS2DW12_CTRL1, LIS2DW12_CTRL4_INT1_PAD_CTRL, LIS2DW12_TAP_THS_X, LIS2DW12_CTRL_REG7, LIS2DW12_CTRL6, };

// List of data ararys to send during config uint8_t *accel_config_cmd[ACCEL_NUM_CONFIG_WRITES] = { accel_config_cmd_0, accel_config_cmd_1, accel_config_cmd_2, accel_config_cmd_3, accel_config_cmd_4, };

// Sizes of data arrays to send during config uint16_t accel_config_size[ACCEL_NUM_CONFIG_WRITES] = { sizeof(accel_config_cmd_0), sizeof(accel_config_cmd_1), sizeof(accel_config_cmd_2), sizeof(accel_config_cmd_3), sizeof(accel_config_cmd_4), };

uint8_t accel_config_read_0[sizeof(accel_config_cmd_0)]; uint8_t accel_config_read_1[sizeof(accel_config_cmd_1)]; uint8_t accel_config_read_2[sizeof(accel_config_cmd_2)]; uint8_t accel_config_read_3[sizeof(accel_config_cmd_3)]; uint8_t accel_config_read_4[sizeof(accel_config_cmd_4)];

uint8_t *accel_config_read[ACCEL_NUM_CONFIG_WRITES] = { accel_config_read_0, accel_config_read_1, accel_config_read_2, accel_config_read_3, accel_config_read_4, };

// Macro Functions ---------------------------------------------------------------------------------

define HAS_MAX_READ_TIME_ELAPSED(start_tick) (HAL_GetTick() - start_tick > ACCEL_MAX_READ_TIME)

// Accessors --------------------------------------------------------------------------------------- bool AccelIsBootupComplete(accel_app_st_t *accel_app) { return ACCEL_STATE_READY == accel_app->state; }

// Functions ---------------------------------------------------------------------------------------

//! \brief singleton that generates an i2c request structure //! \details Will also create a side effect of mutating accel_app's request // as well as changing the handle to point to itself //! \retval request generated static i2c_arb_req_st_t GenerateRequest(accel_app_st_t accel_app, i2c_xfer_type_en_t xfer_t, uint16_t reg_addr, uint8_t reg_size_bytes, uint8_t data, uint16_t data_size, i2c_arb_callback_fp_t CallbackFp) { i2c_arb_req_st_t req = &accel_app->request;

req->xfer_type = xfer_t;
req->device_addr = ACCEL_I2C_DEVICE_ADDRESS;
req->reg_addr = reg_addr;
req->reg_size_bytes = reg_size_bytes;
req->data = data;
req->data_size_bytes = data_size;
req->Callback = CallbackFp;
req->driver = (void*)accel_app;
req->num_retry = ACCEL_NUM_MAX_I2C_XFER_ATTEMPTS;

return req;

}

//! Handles EXTI (freefall and orientation) interrupts //! Begins gathering information to compose the notification message void AccelExtiIsr(accel_app_st_t *accel_app, accel_interrupt_en_t interr_type) { // Only trigger an event if we're ready to process it. Otherwise, ignore if(ACCEL_STATE_READY != accel_app->state) { return; }

// for timing tests with the lis2dw12 accelerometer

ifdef CP_UT

ifdef ACCEL_TIMING

DebugSetTestPin1();

endif

endif

// Starts timer for the entire info gathering operation
accel_app->interrupt_type = interr_type;
accel_app->rx_time_0 = HAL_GetTick();
SwCommunicationEventTrigger(EVT_ACCEL);

}

//! Clears accelerometer state after interrupt analysis static void AccelReset(accel_app_st_t *accel_app) { accel_app->data.orientation = ACCEL_ORIENTATION_UNDEFINED; CLR_ALL_FLAGS(accel_app->data.motions); accel_app->interrupt_type = ACCEL_INTERRUPT_NONE; }

//! \brief Converts lsb's based on full scale and resolution to microgravities (signed) //! \param lsb signed, full_scale and resolution parameters //! \retval mg's signed of what the lsb represents float AccelResolutionToMg(accel_app_st_t *accel_app, int16_t lsb, accel_full_scale_en_t full_scale, bool high_performance_mode, accel_resolution_en_t lp_mode) { float gravity_per_lsb = (accel_app->all_sources.status_dup.sleep_state_ia || !high_performance_mode) && lp_mode == ACCEL_RESOLUTION_LOW_POWER_1 ? // 12 bits, 1/2^12 = 0.244, otherwise 14 bits, 1/2^14 = 0.061 0.244f : 0.061f;

// since we have a left aligned 12/14-bit number, shoved in a 16-bit number, 
// we need to shift based on how many by dividing by 2^4 or 2^2
float lsb_16bit_realignment = (accel_app->all_sources.status_dup.sleep_state_ia ||
                                !high_performance_mode) && 
                                lp_mode == ACCEL_RESOLUTION_LOW_POWER_1 ?
                                0.0625f : 0.25f;

float full_scale_gravities;

switch(full_scale) {
    case ACCEL_FULL_SCALE_2G:
        full_scale_gravities = 4;
        break;
    case ACCEL_FULL_SCALE_4G:
        full_scale_gravities = 8;
        break;
    case ACCEL_FULL_SCALE_8G:
        full_scale_gravities = 16;
        break;
    case ACCEL_FULL_SCALE_16G:
        full_scale_gravities = 32;
        break;
    default:
        // set it as 2g
        full_scale_gravities = 4;
        break;
}

return (float)lsb * lsb_16bit_realignment * full_scale_gravities * gravity_per_lsb;

}

float AccelCalculateMagnitude(const accel_data_st_t *accel_data) { double sum_of_squares = 0;

for(int i = 0; i < ACCEL_NUM_COOR; i++) {
    sum_of_squares += pow((double)accel_data->xyz_accel[i], 2);
}

return (float)sqrt(sum_of_squares);

}

// State Machine -----------------------------------------------------------------------------------

//! \brief Called after device has been detected on I2C bus //! \param None //! \retval None void AccelDeviceReadyComplete(i2c_arb_req_st_t req) { accel_app_st_t accel_app = (accel_app_st_t*)(req->driver);

if(I2C_REQ_STATE_COMPLT == req->state) {
    // Initiate configuration process
    SwCommunicationEventTrigger(EVT_ACCEL);
}
else {
    accel_app->error = ACCEL_ERR_FAILED_TO_BOOT;
    accel_app->state = ACCEL_STATE_READY;
    accel_app->is_booting_up = false;
    SET_FLAG(accel_app->device_app->boot_up_app->err_flags, MC_BOOT_UP_ERR_ACCELEROMETER);
    DeviceBootUpActionComplete(accel_app->device_app);
}

}

void AccelStartBoot(accel_app_st_t *accel_app) { accel_app->is_booting_up = true;

// Check that accelerometer is ready, 1 second timeout
i2c_arb_req_st_t *req = GenerateRequest(accel_app, I2C_IS_DEVICE_READY, 0,
                                        0, NULL, 0,  AccelDeviceReadyComplete);

if(!I2cBusSendRequest(accel_app->i2c_arb_app, req)) {
    accel_app->error = ACCEL_ERR_ARB_QUEUE_FULL;
}

}

//! \brief Callback after a transaction on I2C bus has completed //! \param None //! \retval None void AccelWriteConfigComplete(i2c_arb_req_st_t req) { accel_app_st_t accel_app = (accel_app_st_t*)(req->driver);

if(I2C_REQ_STATE_COMPLT == req->state) {
    accel_app->write_config_step++; // Go to next step if successful

    // Trigger an event that will write to the next config register
    SwCommunicationEventTrigger(EVT_ACCEL);

    // Return to the arbiter and remove the request from the queue
}
else {
    accel_app->error = ACCEL_ERR_CONFIG_WRITE;
    accel_app->state = ACCEL_STATE_READY;
    if(accel_app->is_booting_up){
        accel_app->is_booting_up = false;
        SET_FLAG(accel_app->device_app->boot_up_app->err_flags, MC_BOOT_UP_ERR_ACCELEROMETER);
        DeviceBootUpActionComplete(accel_app->device_app);
    }
}

}

//! Writes configuration data to accelerometer, and triggers an event if I2C bus is busy void AccelWriteConfig(accel_app_st_t *accel_app) { // Attempt to write config data if(ACCEL_NUM_CONFIG_WRITES > accel_app->write_config_step) {

    i2c_arb_req_st_t *req = GenerateRequest(accel_app, I2C_IT_WRITE,
                                            accel_config_addr[accel_app->write_config_step],
                                            ACCEL_REG_SIZE,
                                            accel_config_cmd[accel_app->write_config_step],
                                            accel_config_size[accel_app->write_config_step],
                                            AccelWriteConfigComplete);

    if(!I2cBusSendRequest(accel_app->i2c_arb_app, req)) {
        accel_app->error = ACCEL_ERR_ARB_QUEUE_FULL;
    }
}
else {
    // The PL interrupt is falsely raised during start-up due to the change in conditions
    // The CFG_CLEAR_INT state lets the handler clear this false alarm
    accel_app->write_config_step = 0;
    accel_app->state = ACCEL_STATE_CFG_READ;
    SwCommunicationEventTrigger(EVT_ACCEL);
}

}

//! \brief Callback after a transaction on I2C bus has completed //! \param None //! \retval None void AccelReadConfigComplete(i2c_arb_req_st_t req) { accel_app_st_t accel_app = (accel_app_st_t*)(req->driver);

if(I2C_REQ_STATE_COMPLT != req->state) {
    // Check if out of maximum allocated read time
    if(HAS_MAX_READ_TIME_ELAPSED(accel_app->rx_time_0)) {
        accel_app->state = ACCEL_STATE_READY;
    }
    req->state = I2C_REQ_STATE_COMPLT;
    // Return to the arbiter and remove the request from the queue
}

accel_app->write_config_step++;
// Go to analyzing the orientation
SwCommunicationEventTrigger(EVT_ACCEL);

}

void AccelReadConfig(accel_app_st_t *accel_app) { // Attempt to read config data if(ACCEL_NUM_CONFIG_WRITES > accel_app->write_config_step) {

    i2c_arb_req_st_t *req = GenerateRequest(accel_app, I2C_IT_READ,
                                            accel_config_addr[accel_app->write_config_step],
                                            ACCEL_REG_SIZE,
                                            accel_config_read[accel_app->write_config_step],
                                            accel_config_size[accel_app->write_config_step],
                                            AccelReadConfigComplete);

    if(!I2cBusSendRequest(accel_app->i2c_arb_app, req)) {
        accel_app->error = ACCEL_ERR_ARB_QUEUE_FULL;
    }
}
else {
    // The PL interrupt is falsely raised during start-up due to the change in conditions
    // The CFG_CLEAR_INT state lets the handler clear this false alarm
    accel_app->write_config_step = 0;
    accel_app->state = ACCEL_STATE_READY;
    if(accel_app->is_booting_up){
        accel_app->is_booting_up = false;
        DeviceBootUpActionComplete(accel_app->device_app);
    }
}

}

//! \brief Callback after a transaction on I2C bus has completed //! \param None //! \retval None void AccelReadAllSourcesComplete(i2c_arb_req_st_t req) { accel_app_st_t accel_app = (accel_app_st_t*)(req->driver);

if(I2C_REQ_STATE_COMPLT != req->state) {
    // Check if out of maximum allocated read time
    if(HAS_MAX_READ_TIME_ELAPSED(accel_app->rx_time_0)) {
        accel_app->all_sources = lis2dw12_all_sources_t {
            0x00, 0x00, 0x00, 0x00, 0x00
        }; // set all to unknown
    }

    req->state = I2C_REQ_STATE_COMPLT;
    // Return to the arbiter and remove the request from the queue
}

ifdef ACCEL_TIMING

// for timing tests with the lis2dw12 accelerometer 
DebugClearTestPin1();

endif

accel_app->state = ACCEL_STATE_ANALYZE_ORIENTATION;
// Trigger an event that will go analyze the orientation info we just got
SwCommunicationEventTrigger(EVT_ACCEL);

}

void AccelReadAllSources(accel_app_st_t *accel_app) {

i2c_arb_req_st_t *req = GenerateRequest(accel_app, I2C_IT_READ,
                                        LIS2DW12_STATUS_DUP,
                                        ACCEL_REG_SIZE,
                                        (uint8_t *)(&accel_app->all_sources), // cast it to an address
                                        sizeof(lis2dw12_all_sources_t), // read off 5 bytes
                                        AccelReadAllSourcesComplete);

if(!I2cBusSendRequest(accel_app->i2c_arb_app, req)) {
    accel_app->error = ACCEL_ERR_ARB_QUEUE_FULL;
}

}

//! Analyzes the 6id byte to obtain orientation void AccelAnalyzeAllSources(accel_app_st_t *accel_app) { // TODO RK 09-May-2022: Fix orientation based on actual board layout (don't know how chip is // oriented on board, not sure what nose down, nose up, etc. are); Refer to accelerometer // documentation for info on the SIXD_SRC register's info pg. 24 of // https://www.st.com/resource/en/application_note/an5038-lis2dw12-alwayson-3d-accelerometer-stmicroelectronics.pdf // (a) straight up (b) starboard down (c) starboard up (d) upside down (e) nose up (f) nose down

lis2dw12_sixd_src_t sixd_src = accel_app->all_sources.sixd_src;

if (1 == sixd_src.zh) {
    accel_app->data.orientation = ACCEL_ORIENTATION_UPRIGHT;
} 
else if (1 == sixd_src.zl) {
    accel_app->data.orientation = ACCEL_ORIENTATION_UPSIDE_DOWN;
} 
else if (1 == sixd_src.yh) {
    accel_app->data.orientation = ACCEL_ORIENTATION_ON_RIGHTSIDE;
} 
else if (1 == sixd_src.yl) {
    accel_app->data.orientation = ACCEL_ORIENTATION_ON_LEFTSIDE;
} 
else if (1 == sixd_src.xh) {
    accel_app->data.orientation = ACCEL_ORIENTATION_NOSE_DOWN;
} 
else if (1 == sixd_src.xl) {
    accel_app->data.orientation = ACCEL_ORIENTATION_NOSE_UP;
} 
else {
    accel_app->data.orientation = ACCEL_ORIENTATION_UNKNOWN;
}

if (accel_app->interrupt_type == ACCEL_INTERRUPT_2) {
    if(accel_app-> all_sources.status_dup.sleep_state_ia) {
        accel_app->data.power_mode = ACCEL_POWERMODE_LOW;
    }
    else {
        accel_app->data.power_mode = ACCEL_POWERMODE_HIGH;
    }

    SendAccelPowerNotification(MasterGetConn(), &accel_app->data);
    accel_app->state = ACCEL_STATE_READY;
    AccelReset(accel_app);
    return;
}

SwCommunicationEventTrigger(EVT_ACCEL); // Trigger event to read freefall interrupt register

}

//! \brief Callback after a transaction on I2C bus has completed //! \param None //! \retval None void AccelReadXYZComplete(i2c_arb_req_st_t req) { accel_app_st_t accel_app = (accel_app_st_t*)(req->driver);

if(I2C_REQ_STATE_COMPLT != req->state) {
    // Check if out of maximum allocated read time
    if(HAS_MAX_READ_TIME_ELAPSED(accel_app->rx_time_0)) {
        SET_FLAG(accel_app->data.motions, ACCEL_MOTION_UNKNOWN);
        accel_app->state = ACCEL_STATE_ANALYZE_XYZ;
    }
    req->state = I2C_REQ_STATE_COMPLT;
    // Return to the arbiter and remove the request from the queue
}
// Trigger event to either analyze the XYZ accelerations, or re-attempt read, or move on if
// timed out
SwCommunicationEventTrigger(EVT_ACCEL);

}

//! Reads the X, Y, Z acceleration registers void AccelReadXYZ(accel_app_st_t accel_app) {
// Read acceleration data i2c_arb_req_st_t req = GenerateRequest(accel_app, I2C_IT_READ, LIS2DW12_OUT_X_L, ACCEL_REG_SIZE, (uint8_t*)&accel_app->raw_xyz_array, MEMBER_SIZE(accel_app_st_t, raw_xyz_array), AccelReadXYZComplete);

if(!I2cBusSendRequest(accel_app->i2c_arb_app, req)) {
    accel_app->error = ACCEL_ERR_ARB_QUEUE_FULL;
}

}

//! Checks current XYZ acceleration values against the freefall-to-pickup threshold void AccelAnalyzeXYZ(accel_app_st_t *accel_app) { if (1 == accel_app->all_sources.tap_src.single_tap){ SET_FLAG(accel_app->data.motions, ACCEL_MOTION_TAP); } if (1 == accel_app->all_sources.wake_up_src.wu_ia) { SET_FLAG(accel_app->data.motions, ACCEL_MOTION_WAKEUP); } if (1 == accel_app->all_sources.wake_up_src.ff_ia) { SET_FLAG(accel_app->data.motions, ACCEL_MOTION_FREEFALL); } if (1 == accel_app->all_sources.sixd_src._6d_ia){ SET_FLAG(accel_app->data.motions, ACCEL_MOTION_ORIENTATION_CHANGE); } if (ALL_FLAGS_CLEARED == accel_app->data.motions){ SET_FLAG(accel_app->data.motions, ACCEL_MOTION_UNKNOWN); }

if(accel_app->data.power_mode != ACCEL_POWERMODE_DISABLED) {
    if(accel_app->all_sources.status_dup.sleep_state_ia) {
        accel_app->data.power_mode = ACCEL_POWERMODE_LOW;
    }
    else {
        accel_app->data.power_mode = ACCEL_POWERMODE_HIGH;
    }
}

// If motions was not read, clear acceleration values
if(IS_FLAG_SET(accel_app->data.motions, ACCEL_MOTION_UNKNOWN)) {
    for(int i = 0; i < ACCEL_NUM_COOR; i++) {
        accel_app->data.xyz_accel[i] = 0.0;
    }
}
else {
    // first, we need to concatenate the raw_xyz_array into a separate array (since two registers each)
    int16_t xyz_accel_concat[ACCEL_NUM_COOR];

    for (int i = 0; i < ACCEL_NUM_COOR; ++i) {
        // takes the high register and shifts it by 8 places, then add in low register
        xyz_accel_concat[i] = ((int16_t)accel_app->raw_xyz_array[i*2 + 1] * 256) + (int16_t)accel_app->raw_xyz_array[i*2];
    }

    // convert accelerations from eng. units to mg as per driver example
    float acceleration_mg[ACCEL_NUM_COOR];
    for (int i = 0; i < ACCEL_NUM_COOR; ++i) {
        // 1. the full scale setting (ctrl 6, or accel_config_4)
        // 2. if it's low power (lp) or high performance (ctrl 1, accel_config_cmd_0)
        acceleration_mg[i] = AccelResolutionToMg(accel_app,
                                                 xyz_accel_concat[i], 
                                                 ACCEL_FULL_SCALE_2G, 
                                                 HIGH_PERFORMANCE_MODE,
                                                 ACCEL_RESOLUTION_LOW_POWER_2);
    }

    // Convert accelerations to m/s/s
    for(int i = 0; i < ACCEL_NUM_COOR; i++) {
        accel_app->data.xyz_accel[i] = acceleration_mg[i] * ACCEL_GRAVITY;
    }
}

SAFE_MEMCPY(accel_app->prev_data_buffer, accel_app->data);
// RK 07-Nov-2018: The accelerometer interrupt is triggered once while
// booting up, after about 50 to 150 ms. For now, I'll just let the 
// notification go, because the CP doesn't take any action based on it 
// anyways. The accelerometer boots up along with the CP, so we are 
// confident that a false interrupt that occurs during boot-up won't 
// adversely affect a test.
SendAccelMotionNotification(MasterGetConn(), &accel_app->data);
DpmCollectAccelerometerData(accel_app->data);

// Cancel the test after the PMIs have been reported, so that we have them
// in the last PMI packet produced when cancelling the test.
if(DeviceIsTestInProgress(accel_app->device_app->patient_test)) {
    if (1 == accel_app->all_sources.wake_up_src.ff_ia) {            
        DeviceTestCancelDeviceError(accel_app->device_app, 
                                    DEVICE_RC_ACCEL_FREEFALL);
    }
    else if(accel_app->data.orientation != ACCEL_ORIENTATION_UPRIGHT) {
        DeviceTestCancelDeviceError(accel_app->device_app, 
                                    DEVICE_RC_ACCEL_WRONG_ORIENTATION);
    }
}
// for unit test purposes, we need to be able to read the data after it, so we'll skip the reset

ifndef CP_UT

AccelReset(accel_app);

else

// we have a timing test for unit tests with lis2dw12 accelerometer

ifdef ACCEL_TIMING

DebugClearTestPin1();

endif

endif

}

//! Handles accelerometer events void AccelEventHandler(accel_app_st_t *accel_app) { switch(accel_app->state) { case ACCEL_STATE_PWR_ON: accel_app->state = ACCEL_STATE_CFG_WRITE; // Fall-through case ACCEL_STATE_CFG_WRITE: AccelWriteConfig(accel_app); break;

case ACCEL_STATE_CFG_READ:
    AccelReadConfig(accel_app);
    break;

case ACCEL_STATE_READY:
    accel_app->state = ACCEL_STATE_READ_INTERRUPT; 
    // fall-through

case ACCEL_STATE_READ_INTERRUPT:
    CLR_ALL_FLAGS(accel_app->data.motions);
    AccelReadAllSources(accel_app);
    break;

case ACCEL_STATE_ANALYZE_ORIENTATION:
    accel_app->state = ACCEL_STATE_READ_XYZ;
    AccelAnalyzeAllSources(accel_app);
    break;

case ACCEL_STATE_READ_XYZ:
    accel_app->state = ACCEL_STATE_ANALYZE_XYZ;
    AccelReadXYZ(accel_app);
    break;

case ACCEL_STATE_ANALYZE_XYZ:
    accel_app->state = ACCEL_STATE_READY;
    AccelAnalyzeXYZ(accel_app);
    break;

default: // Do nothing
    break;
}

}

// Manually enable and reenable sleep mode during tests void AccelChangeSleepstate(accel_app_st_t accel_app, uint8_t sleepstate_config) { accel_sleepstate = sleepstate_config; i2c_arb_req_st_t req = GenerateRequest(accel_app, I2C_IT_WRITE, LIS2DW12_WAKE_UP_THS, ACCEL_REG_SIZE, &accel_sleepstate, sizeof(uint8_t), AccelWriteConfigComplete);

accel_app->state = ACCEL_STATE_CFG_READ;
accel_app->is_booting_up = false;
memset(accel_config_read_2, 0, sizeof(accel_config_read_2));

if(accel_sleepstate == ACCEL_SLEEPSTATE_CONFIG_OFF){
    accel_app->data.power_mode = ACCEL_POWERMODE_DISABLED;
}
else {
    accel_app->data.power_mode = ACCEL_POWERMODE_UNKNOWN;
}

if(!I2cBusSendRequest(accel_app->i2c_arb_app, req)) {
    accel_app->error = ACCEL_ERR_ARB_QUEUE_FULL;
}

}

void AccelerometerSelfCheck(device_app_st_t *device_app) { if(device_app->main_accel_app->prev_data_buffer.orientation != ACCEL_ORIENTATION_UPRIGHT) { device_app->eqc_app.eqc_results.accelerometer = ACTION_FAILED_COMPLETED; } else { device_app->eqc_app.eqc_results.accelerometer = ACTION_SUCCESSFUL; }

DeviceSelfCheckActionComplete(device_app);

}

// Initialization ----------------------------------------------------------------------------------

//! Accelerometer initialization process void AccelInit(accel_app_st_t accel_app, i2c_arb_app_st_t arb, device_app_st_t *device_app) { // Initialize variables accel_app->state = ACCEL_STATE_PWR_ON; accel_app->is_booting_up = false; accel_app->error = ACCEL_ERR_NONE; accel_app->interrupt_type = ACCEL_INTERRUPT_NONE; accel_app->data.orientation = ACCEL_ORIENTATION_UNDEFINED; CLR_ALL_FLAGS(accel_app->data.motions); accel_app->device_addr = ACCEL_I2C_DEVICE_ADDRESS; accel_app->i2c_arb_app = arb; accel_app->device_app = device_app; accel_app->write_config_step = 0;

AccelExtiIrqCallback.Callback = AccelExtiIsr;
AccelExtiIrqCallback.CallObject = accel_app;  

// Register event
SwCommunicationEventRegister(EVT_ACCEL, (call_routine_fp_t)AccelEventHandler,
                             (call_obj_t)accel_app);

}

endif

avisconti commented 4 months ago

Sorry @ybastiand I'm confused... Are you using the LIS2DW12 STdC driver or are you using your own driver? The lis2dw12 STdC driver is here on this repo (lis2dw12_reg.c). Ijust tried this our driver that comes with few examples and I used the lis2dw12_orientation.c example and it seems to me that it is working fine.

In particular I used a Nucleo F401RE board with a X-NUCLEO-IKS01A3 shield (which has a lis2dw12 on board). I used that orientation example and I was able to detect all 6 orientations (ZH/ZL/YH/YL/XH/XL) which are correct with the lis2dw12 orientation on the shield.

Can you try that solution?

ybastiand commented 4 months ago

Hello,

I'm using my own driver.

I won't try the lis2dw12 STdC driver for 2 reasons:

I don't have the hardware you mentioned. Even if I did have that hardware, my issue is hard to reproduce. With my driver too I can detect all 6 orientations. It's just that sometimes (pretty rarely), when the accelerometer is horizontal and moved sideways, the orientation reads something else than "upright".

Yves.

From: Armando Visconti @.> Sent: Tuesday, June 18, 2024 05:31 To: STMicroelectronics/lis2dw12-pid @.> Cc: ybastiand @.>; Mention @.> Subject: Re: [STMicroelectronics/lis2dw12-pid] Incorrect orientation with LIS2DW12TR (Issue #2)

Sorry @ybastiandhttps://github.com/ybastiand I'm confused... Are you using the LIS2DW12 STdC driver or are you using your own driver? The lis2dw12 STdC driver is here on this repo (lis2dw12_reg.chttps://github.com/STMicroelectronics/lis2dw12-pid/blob/master/lis2dw12_reg.c). Ijust tried this our driver that comes with few examples and I used the lis2dw12_orientation.chttps://github.com/STMicroelectronics/STMems_Standard_C_drivers/blob/master/lis2dw12_STdC/examples/lis2dw12_orientation.c example and it seems to me that it is working fine.

In particular I used a Nucleo F401RE board with a X-NUCLEO-IKS01A3 shield (which has a lis2dw12 on board). I used that orientation example and I was able to detect all 6 orientations (ZH/ZL/YH/YL/XH/XL) which are correct with the lis2dw12 orientation on the shield.

Can you try that solution?

— Reply to this email directly, view it on GitHubhttps://github.com/STMicroelectronics/lis2dw12-pid/issues/2#issuecomment-2175643049, or unsubscribehttps://github.com/notifications/unsubscribe-auth/AFM4GRVNABZUCBY4YTTXUETZH747DAVCNFSM6AAAAABI4UHYFGVHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMZDCNZVGY2DGMBUHE. You are receiving this because you were mentioned.

avisconti commented 4 months ago

Hello, I'm using my own driver.

Mmmh, sorry I missed it in the first place. So, I guess this is not the correct place for this discussion, as it is meant to be more for STdC drivers issue. Anyway, I can try to reproduce your issue on my environment just in case, and then if successful I'll debug it.

Anyway for sensor suspport you can:

  1. try on https://community.st.com/ and ask questions about MEMS and SENSORS
  2. File a ticket on https://stintbugzilla.st.com/

Armando

avisconti commented 4 months ago

Even if I did have that hardware, my issue is hard to reproduce. With my driver too I can detect all 6 orientations. It's just that sometimes (pretty rarely), when the accelerometer is horizontal and moved sideways, the orientation reads something else than "upright".

By the way, are you always checking 6D_IA on SIXD_SRC register, right?

avisconti commented 4 months ago

mmmmh, it seems that I can reproduce it too (sometimes, after a while, with small side movements):

6D Or. switched to (60) ZH
6D Or. switched to (60) ZH
6D Or. switched to (50) ZL
6D Or. switched to (50) ZL
6D Or. switched to (60) ZH
6D Or. switched to (60) ZH

and sometime

6D Or. switched to (60) ZH
6D Or. switched to (60) ZH
6D Or. switched to (42) XH
6D Or. switched to (42) XH
6D Or. switched to (60) ZH

I will ask internally, but I'm not sure if I'll be able to get any fast response...

avisconti commented 4 months ago

@ybastiand what value do you put in 6d_ths (reg 30h)? And in bw_filt (reg 25h)?

You can try to have a higher angle in 6d_ths and/or a lower bandwitdh filter in bw_filt.. Take a look also at AN5038

ybastiand commented 4 months ago

"By the way, are you always checking 6D_IA on SIXD_SRC register, right?" No, my driver never checks that bit. Should it?

"@ybastiand what value do you put in 6d_ths (reg 30h)? And in bw_filt (reg 25h)?" We set the 6D threshold to 60 degrees. We set bw_filt to ODR/2.

avisconti commented 4 months ago

"By the way, are you always checking 6D_IA on SIXD_SRC register, right?" No, my driver never checks that bit. Should it?

Would be better to change orientation only if that bit is high. But I'm not saying it is the solution. Only a good practise.

"@ybastiand what value do you put in 6d_ths (reg 30h)? And in bw_filt (reg 25h)?" We set the 6D threshold to 60 degrees. We set bw_filt to ODR/2.

OK, bw_filt seems already at lowest val. Try to put angle at 80 degrees. They told me it would improve.

Last thing, but I think you are already doing it, is to set LPASS_ON6D to '1'. There are no other configurations unfortunately. The rest is only hardware...

ybastiand commented 4 months ago

"Try to put angle at 80 degrees. They told me it would improve." I'll give that a shot.

"Last thing, but I think you are already doing it, is to set LPASS_ON6D to '1'." I'm not doing it. I will change that as well.

Thank you for your help.

avisconti commented 4 months ago

Thank you for your help.

No problem. Just let me know if you are able to make it working. I'll cross my finger...

avisconti commented 4 months ago

Ah, they also told me that should happen less frequently decreasing the ODR. What rate are you using now?

avisconti commented 4 months ago

"@ybastiand what value do you put in 6d_ths (reg 30h)? And in bw_filt (reg 25h)?" We set the 6D threshold to 60 degrees. We set bw_filt to ODR/2.

Sorry, my mistake. Try to increase the divisor (to odr/20, the maximum, for example). ODR/2 is too low

ybastiand commented 4 months ago

Ah, they also told me that should happen less frequently decreasing the ODR. What rate are you using now?

High-Performance / Low-Power mode 400/200 Hz

Sorry, my mistake. Try to increase the divisor (to odr/20, the maximum, for example). ODR/2 is too low

OK, I'll give that a shot as well.

ybastiand commented 4 months ago

Sorry, my mistake. Try to increase the divisor (to odr/20, the maximum, for example). ODR/2 is too low

I changed this (and only this) from odr/2 to odr/20. After this hard sideways shaking didn't trigger orientation "on left side" anymore. I still had one orientation "upside down" on shaking hard vertically, but I think that's acceptable. I'll release that FW change to our users and we'll know in several months whether that's the fix.

avisconti commented 4 months ago

Sorry, my mistake. Try to increase the divisor (to odr/20, the maximum, for example). ODR/2 is too low

I changed this (and only this) from odr/2 to odr/20. After this hard sideways shaking didn't trigger orientation "on left side" anymore. I still had one orientation "upside down" on shaking hard vertically, but I think that's acceptable. I'll release that FW change to our users and we'll know in several months whether that's the fix.

OK, I'll close this issue for the moment. You can re-open it or open a new one when you'll have more info.