yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

adis16497: add sensor model autodetection (#13863)

Browse Source 
The sensor comes in three variants with different gyro measurement ranges. This patch allows auto-detection of the model at startup and sets the scale factor associated with the particular model.
This commit is contained in:
Kabir Mohammed
2020-01-07 04:47:41 +05:30
committed by Daniel Agar
parent 7166b867e1
commit 5d110101b5
2 changed files with 55 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

56
src/drivers/imu/adis16497/ADIS16497.cpp
View File

@@ -53,11 +53,15 @@ static constexpr uint8_t CONFIG = 0x0A; // Control, clock and miscellaneous co
static constexpr uint8_t DEC_RATE = 0x0C; // Control, output sample rate decimation
static constexpr uint8_t NULL_CNFG = 0x0E; // Control, automatic bias correction configuration
static constexpr uint8_t SYNC_SCALE = 0x10; // Control, automatic bias correction configuration
static constexpr uint8_t RANG_MDL = 0x12; // Measurement range (model-specific) Identifier TODO use this
static constexpr uint8_t RANG_MDL = 0x12; // Measurement range (model-specific) identifier
static constexpr uint8_t FILTR_BNK_0 = 0x16; // Filter selection
static constexpr uint8_t FILTR_BNK_1 = 0x18; // Filter selection
static constexpr uint16_t PROD_ID_ADIS16497 = 0x4071; // ADIS16497 Identification, device number
static constexpr uint16_t PROD_ID_ADIS16497 = 0x4071; // ADIS16497 device number
static constexpr uint16_t RANG_MDL_1BMLZ = 0b0011; // ADIS16497-1 (±125°/sec)
static constexpr uint16_t RANG_MDL_2BMLZ = 0b0111; // ADIS16497-2 (±450°/sec)
static constexpr uint16_t RANG_MDL_3BMLZ = 0b1111; // ADIS16497-3 (±2000°/sec)
// Stall time between SPI transfers
static constexpr uint8_t T_STALL = 2;
@@ -81,11 +85,9 @@ ADIS16497::ADIS16497(int bus, uint32_t device, enum Rotation rotation) :
_px4_accel.set_device_type(DRV_ACC_DEVTYPE_ADIS16497);
_px4_accel.set_sample_rate(ADIS16497_DEFAULT_RATE);
_px4_accel.set_scale(1.25f * CONSTANTS_ONE_G / 1000.0f); // accel 1.25 mg/LSB
_px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ADIS16497);
_px4_gyro.set_sample_rate(ADIS16497_DEFAULT_RATE);
_px4_gyro.set_scale(math::radians(0.025f)); // gyro 0.025 °/sec/LSB
}
ADIS16497::~ADIS16497()
@@ -247,7 +249,19 @@ ADIS16497::probe()
PX4_DEBUG("PRODUCT: %X", product_id);
if (self_test()) {
return PX4_OK;
// Switch to config page
write_reg16(PAGE_ID, 0x03);
uint16_t model_id = read_reg16(RANG_MDL);
if (set_measurement_range(model_id)) {
return PX4_OK;
} else {
PX4_ERR("probe attempt %d: reading model id failed, resetting", i);
reset();
}
} else {
PX4_ERR("probe attempt %d: self test failed, resetting", i);
@@ -269,7 +283,7 @@ ADIS16497::self_test()
// Switch to configuration page
write_reg16(PAGE_ID, 0x03);
// Self test (globa l command bit 1)
// Self test (global command bit 1)
uint8_t value[2] {};
value[0] = (1 << 1);
write_reg16(GLOB_CMD, (uint16_t)value[0]);
@@ -298,6 +312,36 @@ ADIS16497::self_test()
return true;
}
bool
ADIS16497::set_measurement_range(uint16_t model)
{
_px4_accel.set_scale(1.25f * CONSTANTS_ONE_G / 1000.0f); // 1.25 mg/LSB
_px4_accel.set_range(40.0f * CONSTANTS_ONE_G); // 40g
switch (model) {
case RANG_MDL_1BMLZ:
_px4_gyro.set_scale(math::radians(0.00625f)); // 0.00625 °/sec/LSB
_px4_gyro.set_range(math::radians(125.0f)); // 125 °/s
break;
case RANG_MDL_2BMLZ:
_px4_gyro.set_scale(math::radians(0.025f)); // 0.025 °/sec/LSB
_px4_gyro.set_range(math::radians(450.0f)); // 450 °/s
break;
case RANG_MDL_3BMLZ:
_px4_gyro.set_scale(math::radians(0.1f)); // 0.1 °/sec/LSB
_px4_gyro.set_range(math::radians(2000.0f)); // 2000 °/s
break;
default:
PX4_ERR("RANG_MDL: %#X", model);
return false;
}
return true;
}
uint16_t
ADIS16497::read_reg16(uint8_t reg)
{

5
src/drivers/imu/adis16497/ADIS16497.hpp
View File

@@ -164,6 +164,11 @@ private:
// ADIS16497 onboard self test
bool self_test();
/**
* Set measurement range based on the sensor model.
*/
bool set_measurement_range(uint16_t model);
uint32_t crc32(const uint16_t *data, size_t len) const
{
uint32_t crc = 0xffffffff;