123 lines
3.6 KiB
C
123 lines
3.6 KiB
C
|
|
#include "user_asm330lhh.h"
|
|
|
|
|
|
|
|
stmdev_ctx_t spi1_dev ;
|
|
extern SPI_HandleTypeDef hspi1;
|
|
asm_value asm330;
|
|
|
|
static int32_t platform_write(void *handle, uint8_t reg, const uint8_t *bufp,uint16_t len)
|
|
{
|
|
GPIO_TypeDef *GPIOx;
|
|
uint16_t GPIO_Pin;
|
|
if(handle == &hspi1)
|
|
{
|
|
GPIOx = SPI1_CS_PORT;
|
|
GPIO_Pin= SPI1_CS_PIN;
|
|
}
|
|
HAL_GPIO_WritePin(GPIOx, GPIO_Pin, GPIO_PIN_RESET);
|
|
HAL_SPI_Transmit(handle, ®, 1, 1000);
|
|
HAL_SPI_Transmit(handle, (uint8_t*) bufp, len, 1000);
|
|
HAL_GPIO_WritePin(GPIOx, GPIO_Pin, GPIO_PIN_SET);
|
|
return 0;
|
|
}
|
|
|
|
static int32_t platform_read(void *handle, uint8_t reg, uint8_t *bufp,uint16_t len)
|
|
{
|
|
uint8_t temp=reg;
|
|
temp |= 0x80;
|
|
// GPIO_TypeDef *GPIOx;
|
|
// uint16_t GPIO_Pin;
|
|
// if (handle == &hspi1) {
|
|
// GPIOx = SPI1_CS_PORT;
|
|
// GPIO_Pin = SPI1_CS_PIN;
|
|
// }
|
|
// HAL_GPIO_WritePin(GPIOx, GPIO_Pin, GPIO_PIN_RESET);
|
|
// HAL_SPI_Transmit(handle, &temp, 1, 1000);
|
|
// HAL_SPI_Receive(handle, bufp, len, 1000);
|
|
// HAL_GPIO_WritePin(GPIOx, GPIO_Pin, GPIO_PIN_SET);
|
|
|
|
|
|
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_RESET);
|
|
HAL_SPI_Transmit(handle, &temp, 1, 1000);
|
|
HAL_SPI_Receive(handle, bufp, len, 1000);
|
|
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_SET);
|
|
return 0;
|
|
}
|
|
|
|
void user_dev_init()
|
|
{
|
|
static uint8_t rst;
|
|
uint8_t temp;
|
|
spi1_dev.handle=&hspi1;
|
|
spi1_dev.mdelay=HAL_Delay;
|
|
spi1_dev.write_reg=platform_write;
|
|
spi1_dev.read_reg=platform_read;
|
|
|
|
asm330lhh_device_id_get(&spi1_dev, &temp);
|
|
asm330lhh_reset_set(&spi1_dev, PROPERTY_ENABLE);
|
|
do {
|
|
asm330lhh_reset_get(&spi1_dev, &rst);
|
|
} while (rst);
|
|
/* Start device configuration. */
|
|
asm330lhh_device_conf_set(&spi1_dev, PROPERTY_ENABLE);
|
|
/* Enable Block Data Update */
|
|
asm330lhh_block_data_update_set(&spi1_dev, PROPERTY_ENABLE);
|
|
/* Set Output Data Rate */
|
|
asm330lhh_xl_data_rate_set(&spi1_dev, ASM330LHH_XL_ODR_104Hz);
|
|
asm330lhh_gy_data_rate_set(&spi1_dev, ASM330LHH_GY_ODR_104Hz);
|
|
/* Set full scale */
|
|
asm330lhh_xl_full_scale_set(&spi1_dev, ASM330LHH_2g);
|
|
asm330lhh_gy_full_scale_set(&spi1_dev, ASM330LHH_2000dps);
|
|
|
|
asm330lhh_data_ready_mode_set(&spi1_dev, ASM330LHH_DRDY_PULSED);
|
|
asm330lhh_pin_int1_route_t route_val={0};
|
|
route_val.int1_ctrl.int1_drdy_xl=1;
|
|
asm330lhh_pin_int1_route_set(&spi1_dev, &route_val);
|
|
|
|
/* Configure filtering chain(No aux interface)
|
|
* Accelerometer - LPF1 + LPF2 path
|
|
*/
|
|
// asm330lhh_xl_hp_path_on_out_set(&spi1_dev, ASM330LHH_LP_ODR_DIV_100);
|
|
// asm330lhh_xl_filter_lp2_set(&spi1_dev, PROPERTY_ENABLE);
|
|
}
|
|
|
|
void asm_sample()
|
|
{
|
|
static int16_t data_raw_acceleration[3];
|
|
static int16_t data_raw_angular_rate[3];
|
|
uint8_t reg;
|
|
/* Read output only if new xl value is available */
|
|
asm330lhh_gy_flag_data_ready_get(&spi1_dev, ®);
|
|
if (reg) {
|
|
asm330lhh_angular_rate_raw_get(&spi1_dev, data_raw_angular_rate);
|
|
asm330. x_gyro= 0.001*asm330lhh_from_fs2000dps_to_mdps(
|
|
data_raw_angular_rate[0]);
|
|
asm330. y_gyro = 0.001*asm330lhh_from_fs2000dps_to_mdps(
|
|
data_raw_angular_rate[1]);
|
|
asm330. z_gyro = 0.001*asm330lhh_from_fs2000dps_to_mdps(
|
|
data_raw_angular_rate[2]);
|
|
// angular_rate_mdps[0] = asm330lhh_from_fs2000dps_to_mdps(
|
|
// data_raw_angular_rate[0]);
|
|
// angular_rate_mdps[1] = asm330lhh_from_fs2000dps_to_mdps(
|
|
// data_raw_angular_rate[1]);
|
|
// angular_rate_mdps[2] = asm330lhh_from_fs2000dps_to_mdps(
|
|
// data_raw_angular_rate[2]);
|
|
}
|
|
|
|
asm330lhh_xl_flag_data_ready_get(&spi1_dev, ®);
|
|
if (reg) {
|
|
asm330lhh_acceleration_raw_get(&spi1_dev, data_raw_acceleration);
|
|
asm330.x_acc = 0.0098*asm330lhh_from_fs2g_to_mg(
|
|
data_raw_acceleration[0]);
|
|
asm330.y_acc = 0.0098*asm330lhh_from_fs2g_to_mg(
|
|
data_raw_acceleration[1]);
|
|
asm330.z_acc = 0.0098*asm330lhh_from_fs2g_to_mg(
|
|
data_raw_acceleration[2]);
|
|
|
|
}
|
|
|
|
|
|
}
|