/**
* @brief A function used to configure and init the accelerometer
* @param none
* @retval none
*/
void configure_init_accelerometer(void)
{
//Configure the accelerometer initialization struct
LIS3DSH_InitStruct.Power_Mode_Output_DataRate = LIS3DSH_DATARATE_25; /* Ppower down or /active mode with output data rate 3.125 / 6.25 / 12.5 / 25 / 50 / 100 / 400 / 800 / 1600 HZ */
LIS3DSH_InitStruct.Axes_Enable = LIS3DSH_XYZ_ENABLE; /* Axes enable */
LIS3DSH_InitStruct.Continous_Update = LIS3DSH_ContinousUpdate_Disabled; /* Block or update Low/High registers of data until all data is read */
LIS3DSH_InitStruct.AA_Filter_BW = LIS3DSH_AA_BW_50; /* Choose anti-aliasing filter BW 800 / 400 / 200 / 50 Hz*/
LIS3DSH_InitStruct.Full_Scale = LIS3DSH_FULLSCALE_2; /* Full scale 2 / 4 / 6 / 8 / 16 g */
LIS3DSH_InitStruct.Self_Test = LIS3DSH_SELFTEST_NORMAL; /* Self test */
//Configure the accelerometer interrupt struct
LIS3DSH_IntConfigStruct.Dataready_Interrupt = LIS3DSH_DATA_READY_INTERRUPT_ENABLED; /* Enable/Disable data ready interrupt */
LIS3DSH_IntConfigStruct.Interrupt_signal = LIS3DSH_ACTIVE_HIGH_INTERRUPT_SIGNAL; /* Interrupt Signal Active Low / Active High */
LIS3DSH_IntConfigStruct.Interrupt_type = LIS3DSH_INTERRUPT_REQUEST_PULSED; /* Interrupt type as latched or pulsed */
//Enable the interrupt line
HAL_NVIC_SetPriorityGrouping(NVIC_PRIORITYGROUP_0);
HAL_NVIC_EnableIRQ(EXTI0_IRQn);
HAL_NVIC_SetPriority(EXTI0_IRQn, 0, 1);
//Initialize the accelerometer
LIS3DSH_Init(&LIS3DSH_InitStruct);
//Setting the interrupt configuration of the accelerometer
LIS3DSH_DataReadyInterruptConfig(&LIS3DSH_IntConfigStruct);
}
/**
* @brief Initialize the accelerometer.
* @param None
* @retval None
*/
void Accelerometer_Init(void)
{
LIS3DSH_InitTypeDef accel_init;
LIS3DSH_DRYInterruptConfigTypeDef it_config;
accel_init.Power_Mode_Output_DataRate = LIS3DSH_DATARATE_25; /* Output data 25 times per second (25 Hz) as by requirements */
accel_init.Axes_Enable = LIS3DSH_XYZ_ENABLE; /* Enable x, y and z axes */
accel_init.Continous_Update = LIS3DSH_ContinousUpdate_Disabled; /* Disable continuous update to be able to read the MSB of each reading */
accel_init.AA_Filter_BW = LIS3DSH_AA_BW_50; /* Anti-aliasing filter bandwidth set to 50Hz to remove frequencies above 2 * 25Hz */
accel_init.Full_Scale = LIS3DSH_FULLSCALE_2; /* Use full scale of +/- 2g */
accel_init.Self_Test = LIS3DSH_SELFTEST_NORMAL; /* Self test normal */
/* Initialize LIS3DSH */
LIS3DSH_Init(&accel_init);
it_config.Dataready_Interrupt = LIS3DSH_DATA_READY_INTERRUPT_ENABLED; /* Enable DRY interrupt */
it_config.Interrupt_signal = LIS3DSH_ACTIVE_HIGH_INTERRUPT_SIGNAL; /* Set interrupt to be active high as by requirements */
it_config.Interrupt_type = LIS3DSH_INTERRUPT_REQUEST_PULSED; /* Set interrupt type to be pulsed as by requirements */
/* Set LIS3DSH interrupt configuration */
LIS3DSH_DataReadyInterruptConfig(&it_config);
/* Set priority for EXTI0 IRQ */
HAL_NVIC_SetPriority(EXTI0_IRQn, 0, 0);
/* Enable IRQ for the EXTI Line 0 (LSI3DSH generates INT1 interrupt on GPIO_PIN_0 PE0) */
HAL_NVIC_EnableIRQ(EXTI0_IRQn);
}
void bsp_init() {
//bsp_led_init();
bsp_pwm_config();
bsp_sw_init();
bsp_timer_config();
LIS3DSH_Init();
LIS3DSH_Set_Output(0X47);
}
void accelerometer_init(void) {
LIS3DSH_InitTypeDef LIS3DSH_InitStruct;
/* Set configuration of LIS302DL*/
LIS3DSH_InitStruct.Power_Mode_Output_DataRate = LIS3DSH_DATARATE_100;
LIS3DSH_InitStruct.Axes_Enable = LIS3DSH_X_ENABLE | LIS3DSH_Y_ENABLE | LIS3DSH_Z_ENABLE;
LIS3DSH_InitStruct.Full_Scale = LIS3DSH_FULLSCALE_4;
LIS3DSH_InitStruct.AA_Filter_BW = LIS3DSH_AA_BW_200;
LIS3DSH_InitStruct.Continous_Update = LIS3DSH_ContinousUpdate_Enabled;
LIS3DSH_InitStruct.Self_Test = LIS3DSH_SELFTEST_NORMAL;
LIS3DSH_Init(&LIS3DSH_InitStruct);
/* Enable interrupt when data is ready */
LIS3DSH_DRYInterruptConfigTypeDef DRYInterruptConfig;
DRYInterruptConfig.Dataready_Interrupt = LIS3DSH_DATA_READY_INTERRUPT_ENABLED;
DRYInterruptConfig.Interrupt_signal = LIS3DSH_ACTIVE_HIGH_INTERRUPT_SIGNAL;
DRYInterruptConfig.Interrupt_type = LIS3DSH_INTERRUPT_REQUEST_PULSED;
LIS3DSH_DataReadyInterruptConfig(&DRYInterruptConfig);
/* Set pin as input */
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_StructInit(&GPIO_InitStruct);
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_IN;
GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_0;
GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP;
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_Init(GPIOE, &GPIO_InitStruct);
/* Tell system that you will use PE0 for EXTI_Line0 */
SYSCFG_EXTILineConfig(EXTI_PortSourceGPIOE, EXTI_PinSource0);
/* PE0 is connected to EXTI_Line0 */
EXTI_InitTypeDef EXTI_InitStruct;
EXTI_InitStruct.EXTI_Line = EXTI_Line0;
EXTI_InitStruct.EXTI_LineCmd = ENABLE;
EXTI_InitStruct.EXTI_Mode = EXTI_Mode_Interrupt;
EXTI_InitStruct.EXTI_Trigger = EXTI_Trigger_Rising_Falling;
EXTI_Init(&EXTI_InitStruct);
/* Add IRQ vector to NVIC */
NVIC_InitTypeDef NVIC_InitStruct;
NVIC_InitStruct.NVIC_IRQChannel = EXTI0_IRQn;
NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 0x00;
NVIC_InitStruct.NVIC_IRQChannelSubPriority = 0x00;
NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStruct);
ma_filter_initialize(&filter_x, &cb_x, x_buffer);
ma_filter_initialize(&filter_y, &cb_y, y_buffer);
ma_filter_initialize(&filter_z, &cb_z, z_buffer);
}
//Initialize the accelerometer
void initAccel()
{
LIS3DSH_InitTypeDef accel_init;
accel_init.AA_Filter_BW = LIS3DSH_AA_BW_800; //Datasheet says this is the default value so just going to use it b/c not sure why/how AA filter is needed.
accel_init.Axes_Enable = LIS3DSH_X_ENABLE | LIS3DSH_Y_ENABLE | LIS3DSH_Z_ENABLE; //Need to enable all three axis, bitwise OR to set all three control bits high.
accel_init.Continous_Update = LIS3DSH_ContinousUpdate_Enabled; //Otherwise output registers not updated until MSB and LSB reading (but shouldn't this param be 0 then?)
accel_init.Full_Scale = LIS3DSH_FULLSCALE_2; //Tilt angle application notes say typical value is +/- 2g. Maybe increase later if necessary?
accel_init.Power_Mode_Output_DataRate = LIS3DSH_DATARATE_100; //We will be sampling at 100Hz, so output rate should be the same.
accel_init.Self_Test = LIS3DSH_SELFTEST_NORMAL;// Positive sign self test, negative sign self test, or normal mode self test...lets stick with normal for now.
LIS3DSH_Init(&accel_init);
}
void prvMEMS_Config(void)
{
uint8_t ctrl = 0;
LIS3DSH_InitTypeDef LIS3DSH_InitStruct;
LIS3DSH_InitStruct.SM1_Hysteresis=0x00;
LIS3DSH_InitStruct.SM1_Pin=LIS3DSH_SM1_INT_TO_PIN_INT1;
LIS3DSH_InitStruct.SM1_Enable=LIS3DSH_SM1_DISABLE;
LIS3DSH_InitStruct.SM2_Hysteresis=0x00;
LIS3DSH_InitStruct.SM2_Pin=LIS3DSH_SM2_INT_TO_PIN_INT1;
LIS3DSH_InitStruct.SM2_Enable=LIS3DSH_SM2_DISABLE;
LIS3DSH_InitStruct.CR3_Dren=LIS3DSH_CR3_DREN_TO_INT1_DISABLE;
LIS3DSH_InitStruct.CR3_Iea=LIS3DSH_CR3_IEA_ACTIVE_LOW;
LIS3DSH_InitStruct.CR3_Iel=LIS3DSH_CR3_IEL_LATCHED;
LIS3DSH_InitStruct.CR3_Int2En=LIS3DSH_CR3_INT2_DISABLED;
LIS3DSH_InitStruct.CR3_Int1En=LIS3DSH_CR3_INT1_DISABLED;
LIS3DSH_InitStruct.CR3_Vfilt=LIS3DSH_CR3_VFILT_DISABLED;
LIS3DSH_InitStruct.CR3_Strt=LIS3DSH_CR3_NO_SOFT_RESET;
LIS3DSH_InitStruct.CR4_Odr=LIS3DSH_CR4_ODR_100HZ;
LIS3DSH_InitStruct.CR4_Bdu=LIS3DSH_CR4_BDU_ENABLED;
LIS3DSH_InitStruct.CR4_Zen=LIS3DSH_CR4_Z_AXIS_ENABLED;
LIS3DSH_InitStruct.CR4_Yen=LIS3DSH_CR4_Y_AXIS_ENABLED;
LIS3DSH_InitStruct.CR4_Xen=LIS3DSH_CR4_X_AXIS_ENABLED;
LIS3DSH_InitStruct.CR5_Bw=LIS3DSH_CR5_BW_50HZ;
LIS3DSH_InitStruct.CR5_Fscale=LIS3DSH_CR5_FSCALE_2G;
LIS3DSH_InitStruct.CR5_St=LIS3DSH_CR5_ST_DISABLE;
LIS3DSH_InitStruct.CR5_Sim=LIS3DSH_CR5_MODE_4_WIRE_INTERFACE;
LIS3DSH_InitStruct.CR6_Boot=LIS3DSH_CR6_FORCE_REBOOT_DISABLE;
LIS3DSH_InitStruct.CR6_FifoEn=LIS3DSH_CR6_FIFO_DISABLED;
LIS3DSH_InitStruct.CR6_WtmEn=LIS3DSH_CR6_WTM_DISABLED;
LIS3DSH_InitStruct.CR6_AddInc=LIS3DSH_CR6_ADDINC_DISABLED;
LIS3DSH_InitStruct.CR6_P1Empty=LIS3DSH_CR6_FIFO_EMPTY_TO_INT1_DISABLED;
LIS3DSH_InitStruct.CR6_P1Wtm=LIS3DSH_CR6_FIFO_WTM_TO_INT1_DISABLED;
LIS3DSH_InitStruct.CR6_P1OverRun=LIS3DSH_CR6_FIFO_OVERRUN_TO_INT1_DISABLED;
LIS3DSH_InitStruct.CR6_P2Boot=LIS3DSH_CR6_BOOT_TO_INT2_DISABLED;
LIS3DSH_Init(&LIS3DSH_InitStruct);
}
/*Configure the accelerometer
*/
void Configure_Accel(void)
{
LIS3DSH_InitStruct.Power_Mode_Output_DataRate = LIS3DSH_DATARATE_25;
LIS3DSH_InitStruct.Axes_Enable = LIS3DSH_X_ENABLE | LIS3DSH_Y_ENABLE | LIS3DSH_Z_ENABLE;
LIS3DSH_InitStruct.Full_Scale = LIS3DSH_FULLSCALE_4;
LIS3DSH_InitStruct.AA_Filter_BW = LIS3DSH_AA_BW_200;
LIS3DSH_InitStruct.Continous_Update = LIS3DSH_ContinousUpdate_Disabled;
LIS3DSH_InitStruct.Self_Test = LIS3DSH_SELFTEST_NORMAL;
DRYInterruptConfig.Dataready_Interrupt = LIS3DSH_DATA_READY_INTERRUPT_ENABLED;
DRYInterruptConfig.Interrupt_signal = LIS3DSH_ACTIVE_HIGH_INTERRUPT_SIGNAL;
DRYInterruptConfig.Interrupt_type = LIS3DSH_INTERRUPT_REQUEST_PULSED;
LIS3DSH_DataReadyInterruptConfig(&DRYInterruptConfig);
LIS3DSH_Init(&LIS3DSH_InitStruct);
HAL_NVIC_SetPriority(EXTI0_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(EXTI0_IRQn);
}
void memsinit(void){
LIS3DSH_InitTypeDef meminit;
LIS3DSH_DRYInterruptConfigTypeDef intinit;
meminit.Continous_Update = LIS3DSH_ContinousUpdate_Disabled;
meminit.Power_Mode_Output_DataRate = LIS3DSH_DATARATE_25;
meminit.AA_Filter_BW = LIS3DSH_AA_BW_800;
meminit.Axes_Enable = LIS3DSH_XYZ_ENABLE;
meminit.Full_Scale = LIS3DSH_FULLSCALE_2;
meminit.Self_Test = LIS3DSH_SELFTEST_NORMAL;
LIS3DSH_Init(&meminit);
intinit.Dataready_Interrupt = LIS3DSH_DATA_READY_INTERRUPT_ENABLED;
intinit.Interrupt_signal = LIS3DSH_ACTIVE_HIGH_INTERRUPT_SIGNAL;
intinit.Interrupt_type = LIS3DSH_INTERRUPT_REQUEST_PULSED;
LIS3DSH_DataReadyInterruptConfig(&intinit);
}
/**
* @brief Initialise the Accelerometer
* @param None
* @retval None
*/
void Accelerometer_Config(void) {
/* initialise accelerometer */
LIS3DSH_InitTypeDef Acc_InitDef;
/* define field of the accelerometer initialisation structure */
Acc_InitDef.Power_Mode_Output_DataRate = LIS3DSH_DATARATE_25; /* 25Hz */
Acc_InitDef.Axes_Enable = LIS3DSH_XYZ_ENABLE; /* XYZ */
Acc_InitDef.Continous_Update = LIS3DSH_ContinousUpdate_Disabled; /* continuous update */
Acc_InitDef.AA_Filter_BW = LIS3DSH_AA_BW_50; /* 50Hz to filter gravity*/
Acc_InitDef.Full_Scale = LIS3DSH_FULLSCALE_2; /* 2g */
LIS3DSH_Init(&Acc_InitDef);
/* Init Kalman Filter for the accelerometer */
xState = malloc(sizeof(kalmanState));
yState = malloc(sizeof(kalmanState));
zState = malloc(sizeof(kalmanState));
kalmanInit(xState, INIT_q, INIT_r, INIT_x, INIT_p, INIT_k);
kalmanInit(yState, INIT_q, INIT_r, INIT_y, INIT_p, INIT_k);
kalmanInit(zState, INIT_q, INIT_r, INIT_z, INIT_p, INIT_k);
}
