代码拉取完成，页面将自动刷新
/**
* @file adis16505.c
* @brief Source file for the ADIS16505 inertial sensor driver.
*
* This source file contains the implementation of the functions declared in adis16505.h.
* It handles the low-level communication with the ADIS16505 sensor and provides an API
* for accessing its data.
*
* @author Neo
* @date March, 2024
* @version V2.0
*
* @see adis16505.h for function declarations and type definitions.
*/
#include "adis16505.h"
#include <string.h>
/* Declaration of static functions. */
static void imu_w_reg(uint8_t address, uint16_t w_data);
static uint16_t imu_r_reg(uint8_t address);
#define RXBUF_LEN 16 // number of words (16bit) in rxBuf
// all read data will store in this array
static uint16_t rxBuf[RXBUF_LEN] = {0, }; // shared variable
/*
* @brief init ADIS16505 struct
* */
void ADIS16505_StructInit(ADIS16505 *pImu)
{
// default value
pImu->prop.Gyro_sens_32bit = GYRO_SENSITIVITY_2;
pImu->prop.Gyro_sens_16bit = GYRO_SENSITIVITY_2/65536.0;
pImu->prop.Accel_sens_32bit = ACCL_SENSITIVITY;
pImu->prop.Accel_sens_16bit = ACCL_SENSITIVITY/65536.0;
for (uint8_t i = 0; i < 3; i++)
{
pImu->data.Gyro[i] = 0;
pImu->data.Accel[i] = 0;
pImu->data.Gyro_raw[i] = 0;
pImu->data.Accel_raw[i] = 0;
pImu->prop.Gyro_bias[i] = 0;
pImu->prop.Accel_bias[i] = 0;
}
}
/*
* @brief read ADIS16505 PROD_ID reg
* @return ADIS16505 PROD_ID value, expected to be 0x4079 (16505)
* */
uint16_t ADIS16505_ReadID(ADIS16505 *pImu)
{
uint16_t prod_id = imu_r_reg(PROD_ID);
return prod_id;
}
/*
* @brief read ADIS16505 Gyroscope Bias
* Gyro_raw = Original value + Gyro_bias
* */
void ADIS16505_ReadGyroBias(ADIS16505 *pImu)
{
uint16_t xg_b_l = imu_r_reg(XG_BIAS_LOW);
uint16_t xg_b_h = imu_r_reg(XG_BIAS_HIGH);
uint16_t yg_b_l = imu_r_reg(YG_BIAS_LOW);
uint16_t yg_b_h = imu_r_reg(YG_BIAS_HIGH);
uint16_t zg_b_l = imu_r_reg(ZG_BIAS_LOW);
uint16_t zg_b_h = imu_r_reg(ZG_BIAS_HIGH);
pImu->prop.Gyro_bias[0] = ((int32_t)xg_b_h << 16) | xg_b_l;
pImu->prop.Gyro_bias[1] = ((int32_t)yg_b_h << 16) | yg_b_l;
pImu->prop.Gyro_bias[2] = ((int32_t)zg_b_h << 16) | zg_b_l;
}
/*
* @brief read ADIS16505 Accelerometer Bias
* Accel_raw = Original value + Accel_bias
* */
void ADIS16505_ReadAccelBias(ADIS16505 *pImu)
{
uint16_t xa_b_l = imu_r_reg(XA_BIAS_LOW);
uint16_t xa_b_h = imu_r_reg(XA_BIAS_HIGH);
uint16_t ya_b_l = imu_r_reg(YA_BIAS_LOW);
uint16_t ya_b_h = imu_r_reg(YA_BIAS_HIGH);
uint16_t za_b_l = imu_r_reg(ZA_BIAS_LOW);
uint16_t za_b_h = imu_r_reg(ZA_BIAS_HIGH);
pImu->prop.Accel_bias[0] = ((int32_t)xa_b_h << 16) | xa_b_l;
pImu->prop.Accel_bias[1] = ((int32_t)ya_b_h << 16) | ya_b_l;
pImu->prop.Accel_bias[2] = ((int32_t)za_b_h << 16) | za_b_l;
}
/*
* @brief init ADIS16505
* */
uint8_t ADIS16505_Init(ADIS16505 *pImu)
{
ADIS16505_StructInit(pImu);
// reset ADIS16505
#ifdef ADIS16505_USE_HARDWARE_RST
ADIS16505_HardwareReset();
#else
// Assume SPI communication is working correctly
imu_w_reg(GLOB_CMD, 0x0080); // reset ADIS16505 (Bit[7]: Software reset)
delay_ms(300); // at least 255ms
#endif /* ADIS16505_USE_HARDWARE_RST */
// check connection
if(16505 != ADIS16505_ReadID(pImu))
{
return 1; // connection fail
}
uint16_t rang_mdl = imu_r_reg(RANG_MDL); // read ADIS16505 type
// set gyro sensitivity
switch ((uint8_t)(rang_mdl>>2))
{
case 0: // 0b00 -> ADIS16505-1BMLZ
pImu->prop.Gyro_sens_32bit = GYRO_SENSITIVITY_1;
pImu->prop.Gyro_sens_16bit = GYRO_SENSITIVITY_1/65536.0;
break;
case 3: // 0b11 -> ADIS16505-3BMLZ
pImu->prop.Gyro_sens_32bit = GYRO_SENSITIVITY_3;
pImu->prop.Gyro_sens_16bit = GYRO_SENSITIVITY_3/65536.0;
break;
default: // 0b01 -> ADIS16505-2BMLZ
pImu->prop.Gyro_sens_32bit = GYRO_SENSITIVITY_2;
pImu->prop.Gyro_sens_16bit = GYRO_SENSITIVITY_2/65536.0;
break;
}
/* MISCELLANEOUS */
// bit[9] = 1: 32-bit burst enable (BURST32 = 1)
// bit[7] = 1: enable the factory calibration of the gyroscopes and the accelerometer
// others: use default
#ifdef USE_INPUT_SYNC_MODE
imu_w_reg(MSC_CTRL, 0x02C4); // Direct Input Sync Mode
#else
imu_w_reg(MSC_CTRL, 0x02C0); // Internal Clock Mode
#endif /* USE_INPUT_SYNC_MODE */
// delay_ms(250); // uncomment this line if SENS_BW changed
/* BARTLETT WINDOW FILTER */
imu_w_reg(FILT_CTRL, 0x0003); // set bartlett window filter (N = 2^3)
/* DECIMATION FILTER */
imu_w_reg(DEC_RATE, 0x0013); // the nominal output data rate is equal to 2000/(DEC_RATE + 1) = 100Hz (internal clock mode)
delay_ms(100); // wait for data ready
return 0;
}
/*
* @brief read gyro original value and calculate real value
* data will be stored in pImu
* */
void ADIS16505_ReadGyro(ADIS16505 *pImu)
{
uint16_t x_g_l = imu_r_reg(X_GYRO_LOW);
uint16_t x_g_o = imu_r_reg(X_GYRO_OUT);
uint16_t y_g_l = imu_r_reg(Y_GYRO_LOW);
uint16_t y_g_o = imu_r_reg(Y_GYRO_OUT);
uint16_t z_g_l = imu_r_reg(Z_GYRO_LOW);
uint16_t z_g_o = imu_r_reg(Z_GYRO_OUT);
// calculate x axis
pImu->data.Gyro_raw[0] = ((int32_t)x_g_o << 16) | x_g_l;
pImu->data.Gyro[0] = pImu->data.Gyro_raw[0] / pImu->prop.Gyro_sens_32bit;
// calculate y axis
pImu->data.Gyro_raw[1] = ((int32_t)y_g_o << 16) | y_g_l;
pImu->data.Gyro[1] = pImu->data.Gyro_raw[1] / pImu->prop.Gyro_sens_32bit;
// calculate z axis
pImu->data.Gyro_raw[2] = ((int32_t)z_g_o << 16) | z_g_l;
pImu->data.Gyro[2] = pImu->data.Gyro_raw[2] / pImu->prop.Gyro_sens_32bit;
}
/*
* @brief read accel original value and calculate real value
* data will be stored in pImu
* */
void ADIS16505_ReadAccel(ADIS16505 *pImu)
{
uint16_t x_a_l = imu_r_reg(X_ACCL_LOW);
uint16_t x_a_o = imu_r_reg(X_ACCL_OUT);
uint16_t y_a_l = imu_r_reg(Y_ACCL_LOW);
uint16_t y_a_o = imu_r_reg(Y_ACCL_OUT);
uint16_t z_a_l = imu_r_reg(Z_ACCL_LOW);
uint16_t z_a_o = imu_r_reg(Z_ACCL_OUT);
// calculate x axis
pImu->data.Accel_raw[0] = ((int32_t)x_a_o << 16) | x_a_l;
pImu->data.Accel[0] = pImu->data.Accel_raw[0] / pImu->prop.Accel_sens_32bit;
// calculate y axis
pImu->data.Accel_raw[1] = ((int32_t)y_a_o << 16) | y_a_l;
pImu->data.Accel[1] = pImu->data.Accel_raw[1] / pImu->prop.Accel_sens_32bit;
// calculate z axis
pImu->data.Accel_raw[2] = ((int32_t)z_a_o << 16) | z_a_l;
pImu->data.Accel[2] = pImu->data.Accel_raw[2] / pImu->prop.Accel_sens_32bit;
}
/*
* @brief read temparature original value and calculate real value
* data will be stored in pImu
* */
void ADIS16505_ReadTemp(ADIS16505 *pImu)
{
uint16_t temperature = imu_r_reg(TEMP_OUT);
pImu->data.Temp = (float)temperature * TEMP_SCALE_FACTOR;
}
/*
* @brief burst read original value and calculate real value
* data will be stored in pImu
* use 32-Bit Burst Mode with BURST_SEL = 0 as default (see "MISCELLANEOUS" in ADIS16505_Init())
* */
uint8_t ADIS16505_BurstRead(ADIS16505 *pImu)
{
ADIS16505_Select();
uint16_t dummyBuf[16] = {0, };
memset(rxBuf, 0, sizeof(rxBuf)); // for debug & safety
SPI_ADIS16505_WR_Word(BURST_CMD, rxBuf);
SPI_ADIS16505_WR_Words(dummyBuf, rxBuf, 16);
ADIS16505_Deselect();
delay_us(tSTALL);
if (0 == rxBuf[0]) // DIAG_STAT (default 0x0000)
{
uint16_t checksum = 0;
for(uint8_t i = 0; i < 15; i++)
{
checksum += ((rxBuf[i] >> 8) + (rxBuf[i] & 0x00FF));
}
if (checksum == rxBuf[15]) // checksum correct
{
/* Gyro */
uint16_t x_g_l = rxBuf[1];
uint16_t x_g_o = rxBuf[2];
uint16_t y_g_l = rxBuf[3];
uint16_t y_g_o = rxBuf[4];
uint16_t z_g_l = rxBuf[5];
uint16_t z_g_o = rxBuf[6];
/* Accel */
uint16_t x_a_l = rxBuf[7];
uint16_t x_a_o = rxBuf[8];
uint16_t y_a_l = rxBuf[9];
uint16_t y_a_o = rxBuf[10];
uint16_t z_a_l = rxBuf[11];
uint16_t z_a_o = rxBuf[12];
/* Temp */
uint16_t temperature = rxBuf[13];
/* Gyro */
// calculate x axis
pImu->data.Gyro_raw[0] = ((int32_t)x_g_o << 16) | x_g_l;
pImu->data.Gyro[0] = pImu->data.Gyro_raw[0] / pImu->prop.Gyro_sens_32bit;
// calculate y axis
pImu->data.Gyro_raw[1] = ((int32_t)y_g_o << 16) | y_g_l;
pImu->data.Gyro[1] = pImu->data.Gyro_raw[1] / pImu->prop.Gyro_sens_32bit;
// calculate z axis
pImu->data.Gyro_raw[2] = ((int32_t)z_g_o << 16) | z_g_l;
pImu->data.Gyro[2] = pImu->data.Gyro_raw[2] / pImu->prop.Gyro_sens_32bit;
/* Accel */
// calculate x axis
pImu->data.Accel_raw[0] = ((int32_t)x_a_o << 16) | x_a_l;
pImu->data.Accel[0] = pImu->data.Accel_raw[0] / pImu->prop.Accel_sens_32bit;
// calculate y axis
pImu->data.Accel_raw[1] = ((int32_t)y_a_o << 16) | y_a_l;
pImu->data.Accel[1] = pImu->data.Accel_raw[1] / pImu->prop.Accel_sens_32bit;
// calculate z axis
pImu->data.Accel_raw[2] = ((int32_t)z_a_o << 16) | z_a_l;
pImu->data.Accel[2] = pImu->data.Accel_raw[2] / pImu->prop.Accel_sens_32bit;
/* Temp */
pImu->data.Temp = temperature * TEMP_SCALE_FACTOR;
}
else
return 2; // checksum incorrect
}
else
return 1; // DIAG_STAT error
return 0; // burst read success
}
/*** basic ADIS16505 operation ***/
/*
* @brief write ADIS16505 reg through spi
* @param address: address of the first reg to write
* @param w_data: data to write
* */
static void imu_w_reg(uint8_t address, uint16_t w_data)
{
uint8_t w_bytes[2] = {w_data & 0xFF, w_data >> 8}; // low byte first, high byte second
ADIS16505_Select();
// "|0x80": the write bit (Bit[15]: R*/W = 1)
uint16_t txBuf[2] = {
((address | 0x80) << 8) | w_bytes[0], // low byte
(((address + 1) | 0x80) << 8) | w_bytes[1] // high byte
};
SPI_ADIS16505_WR_Words(txBuf, rxBuf, 2);
ADIS16505_Deselect();
delay_us(tSTALL);
}
/*
* @brief read ADIS16505 regs through spi
* @param address: address of reg to write
* @return read bytes array
* */
static uint16_t imu_r_reg(uint8_t address)
{
ADIS16505_Select();
SPI_ADIS16505_WR_Word((uint16_t)address << 8, rxBuf);
ADIS16505_Deselect();
delay_us(tSTALL);
ADIS16505_Select();
SPI_ADIS16505_WR_Word(0, rxBuf); // receive reply of previous request
ADIS16505_Deselect();
delay_us(tSTALL);
return rxBuf[0];
}
此处可能存在不合适展示的内容，页面不予展示。您可通过相关编辑功能自查并修改。
如您确认内容无涉及 不当用语 / 纯广告导流 / 暴力 / 低俗色情 / 侵权 / 盗版 / 虚假 / 无价值内容或违法国家有关法律法规的内容，可点击提交进行申诉，我们将尽快为您处理。
马建仓 AI 助手