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

drivers/imu/invensense: simplify gyro range configuration

Browse Source
This commit is contained in:
Daniel Agar
2020-06-12 11:20:43 -04:00
parent 1efc8c27eb
commit daf75e40eb
10 changed files with 92 additions and 84 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
src/drivers/imu/invensense/icm20602/ICM20602.cpp
View File

@@ -276,27 +276,28 @@ void ICM20602::ConfigureGyro()
{ {
const uint8_t FS_SEL = RegisterRead(Register::GYRO_CONFIG) & (Bit4 | Bit3); // [4:3] FS_SEL[1:0] const uint8_t FS_SEL = RegisterRead(Register::GYRO_CONFIG) & (Bit4 | Bit3); // [4:3] FS_SEL[1:0]
float range_dps = 0.f;
switch (FS_SEL) { switch (FS_SEL) {
case FS_SEL_250_DPS: case FS_SEL_250_DPS:
_px4_gyro.set_scale(math::radians(1.f / 131.f)); range_dps = 250.f;
_px4_gyro.set_range(math::radians(250.f));
break; break;
case FS_SEL_500_DPS: case FS_SEL_500_DPS:
_px4_gyro.set_scale(math::radians(1.f / 65.5f)); range_dps = 500.f;
_px4_gyro.set_range(math::radians(500.f));
break; break;
case FS_SEL_1000_DPS: case FS_SEL_1000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 32.8f)); range_dps = 1000.f;
_px4_gyro.set_range(math::radians(1000.f));
break; break;
case FS_SEL_2000_DPS: case FS_SEL_2000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 16.4f)); range_dps = 2000.f;
_px4_gyro.set_range(math::radians(2000.f));
break; break;
} }
_px4_gyro.set_scale(math::radians(range_dps / 32768.f));
_px4_gyro.set_range(math::radians(range_dps));
} }
void ICM20602::ConfigureSampleRate(int sample_rate) void ICM20602::ConfigureSampleRate(int sample_rate)

17
src/drivers/imu/invensense/icm20608g/ICM20608G.cpp
View File

@@ -309,27 +309,28 @@ void ICM20608G::ConfigureGyro()
{ {
const uint8_t FS_SEL = RegisterRead(Register::GYRO_CONFIG) & (Bit4 | Bit3); // [4:3] FS_SEL[1:0] const uint8_t FS_SEL = RegisterRead(Register::GYRO_CONFIG) & (Bit4 | Bit3); // [4:3] FS_SEL[1:0]
float range_dps = 0.f;
switch (FS_SEL) { switch (FS_SEL) {
case FS_SEL_250_DPS: case FS_SEL_250_DPS:
_px4_gyro.set_scale(math::radians(1.f / 131.f)); range_dps = 250.f;
_px4_gyro.set_range(math::radians(250.f));
break; break;
case FS_SEL_500_DPS: case FS_SEL_500_DPS:
_px4_gyro.set_scale(math::radians(1.f / 65.5f)); range_dps = 500.f;
_px4_gyro.set_range(math::radians(500.f));
break; break;
case FS_SEL_1000_DPS: case FS_SEL_1000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 32.8f)); range_dps = 1000.f;
_px4_gyro.set_range(math::radians(1000.f));
break; break;
case FS_SEL_2000_DPS: case FS_SEL_2000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 16.4f)); range_dps = 2000.f;
_px4_gyro.set_range(math::radians(2000.f));
break; break;
} }
_px4_gyro.set_scale(math::radians(range_dps / 32768.f));
_px4_gyro.set_range(math::radians(range_dps));
} }
void ICM20608G::ConfigureSampleRate(int sample_rate) void ICM20608G::ConfigureSampleRate(int sample_rate)

17
src/drivers/imu/invensense/icm20649/ICM20649.cpp
View File

@@ -291,27 +291,28 @@ void ICM20649::ConfigureGyro()
{ {
const uint8_t GYRO_FS_SEL = RegisterRead(Register::BANK_2::GYRO_CONFIG_1) & (Bit2 | Bit1); // 2:1 GYRO_FS_SEL[1:0] const uint8_t GYRO_FS_SEL = RegisterRead(Register::BANK_2::GYRO_CONFIG_1) & (Bit2 | Bit1); // 2:1 GYRO_FS_SEL[1:0]
float range_dps = 0.f;
switch (GYRO_FS_SEL) { switch (GYRO_FS_SEL) {
case GYRO_FS_SEL_500_DPS: case GYRO_FS_SEL_500_DPS:
_px4_gyro.set_scale(math::radians(1.f / 65.5f)); range_dps = 500.f;
_px4_gyro.set_range(math::radians(500.f));
break; break;
case GYRO_FS_SEL_1000_DPS: case GYRO_FS_SEL_1000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 32.8f)); range_dps = 1000.f;
_px4_gyro.set_range(math::radians(1000.f));
break; break;
case GYRO_FS_SEL_2000_DPS: case GYRO_FS_SEL_2000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 16.4f)); range_dps = 2000.f;
_px4_gyro.set_range(math::radians(2000.f));
break; break;
case GYRO_FS_SEL_4000_DPS: case GYRO_FS_SEL_4000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 8.2f)); range_dps = 4000.f;
_px4_gyro.set_range(math::radians(4000.f));
break; break;
} }
_px4_gyro.set_scale(math::radians(range_dps / 32768.f));
_px4_gyro.set_range(math::radians(range_dps));
} }
void ICM20649::ConfigureSampleRate(int sample_rate) void ICM20649::ConfigureSampleRate(int sample_rate)

17
src/drivers/imu/invensense/icm20689/ICM20689.cpp
View File

@@ -309,27 +309,28 @@ void ICM20689::ConfigureGyro()
{ {
const uint8_t FS_SEL = RegisterRead(Register::GYRO_CONFIG) & (Bit4 | Bit3); // [4:3] FS_SEL[1:0] const uint8_t FS_SEL = RegisterRead(Register::GYRO_CONFIG) & (Bit4 | Bit3); // [4:3] FS_SEL[1:0]
float range_dps = 0.f;
switch (FS_SEL) { switch (FS_SEL) {
case FS_SEL_250_DPS: case FS_SEL_250_DPS:
_px4_gyro.set_scale(math::radians(1.f / 131.f)); range_dps = 250.f;
_px4_gyro.set_range(math::radians(250.f));
break; break;
case FS_SEL_500_DPS: case FS_SEL_500_DPS:
_px4_gyro.set_scale(math::radians(1.f / 65.5f)); range_dps = 500.f;
_px4_gyro.set_range(math::radians(500.f));
break; break;
case FS_SEL_1000_DPS: case FS_SEL_1000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 32.8f)); range_dps = 1000.f;
_px4_gyro.set_range(math::radians(1000.f));
break; break;
case FS_SEL_2000_DPS: case FS_SEL_2000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 16.4f)); range_dps = 2000.f;
_px4_gyro.set_range(math::radians(2000.f));
break; break;
} }
_px4_gyro.set_scale(math::radians(range_dps / 32768.f));
_px4_gyro.set_range(math::radians(range_dps));
} }
void ICM20689::ConfigureSampleRate(int sample_rate) void ICM20689::ConfigureSampleRate(int sample_rate)

17
src/drivers/imu/invensense/icm20948/ICM20948.cpp
View File

@@ -325,27 +325,28 @@ void ICM20948::ConfigureGyro()
{ {
const uint8_t GYRO_FS_SEL = RegisterRead(Register::BANK_2::GYRO_CONFIG_1) & (Bit2 | Bit1); // 2:1 GYRO_FS_SEL[1:0] const uint8_t GYRO_FS_SEL = RegisterRead(Register::BANK_2::GYRO_CONFIG_1) & (Bit2 | Bit1); // 2:1 GYRO_FS_SEL[1:0]
float range_dps = 0.f;
switch (GYRO_FS_SEL) { switch (GYRO_FS_SEL) {
case GYRO_FS_SEL_250_DPS: case GYRO_FS_SEL_250_DPS:
_px4_gyro.set_scale(math::radians(1.f / 131.f)); range_dps = 250.f;
_px4_gyro.set_range(math::radians(250.f));
break; break;
case GYRO_FS_SEL_500_DPS: case GYRO_FS_SEL_500_DPS:
_px4_gyro.set_scale(math::radians(1.f / 65.5f)); range_dps = 500.f;
_px4_gyro.set_range(math::radians(500.f));
break; break;
case GYRO_FS_SEL_1000_DPS: case GYRO_FS_SEL_1000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 32.8f)); range_dps = 1000.f;
_px4_gyro.set_range(math::radians(1000.f));
break; break;
case GYRO_FS_SEL_2000_DPS: case GYRO_FS_SEL_2000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 16.4f)); range_dps = 2000.f;
_px4_gyro.set_range(math::radians(2000.f));
break; break;
} }
_px4_gyro.set_scale(math::radians(range_dps / 32768.f));
_px4_gyro.set_range(math::radians(range_dps));
} }
void ICM20948::ConfigureSampleRate(int sample_rate) void ICM20948::ConfigureSampleRate(int sample_rate)

20
src/drivers/imu/invensense/icm40609d/ICM40609D.cpp
View File

@@ -277,32 +277,32 @@ void ICM40609D::ConfigureGyro()
{ {
const uint8_t GYRO_FS_SEL = RegisterRead(Register::BANK_0::GYRO_CONFIG0) & (Bit7 | Bit6 | Bit5); // 7:5 GYRO_FS_SEL const uint8_t GYRO_FS_SEL = RegisterRead(Register::BANK_0::GYRO_CONFIG0) & (Bit7 | Bit6 | Bit5); // 7:5 GYRO_FS_SEL
float range_dps = 0.f;
switch (GYRO_FS_SEL) { switch (GYRO_FS_SEL) {
case GYRO_FS_SEL_125_DPS: case GYRO_FS_SEL_125_DPS:
_px4_gyro.set_scale(math::radians(1.f / 262.f)); range_dps = 125.f;
_px4_gyro.set_range(math::radians(125.f));
break; break;
case GYRO_FS_SEL_250_DPS: case GYRO_FS_SEL_250_DPS:
_px4_gyro.set_scale(math::radians(1.f / 131.f)); range_dps = 250.f;
_px4_gyro.set_range(math::radians(250.f));
break; break;
case GYRO_FS_SEL_500_DPS: case GYRO_FS_SEL_500_DPS:
_px4_gyro.set_scale(math::radians(1.f / 65.5f)); range_dps = 500.f;
_px4_gyro.set_range(math::radians(500.f));
break; break;
case GYRO_FS_SEL_1000_DPS: case GYRO_FS_SEL_1000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 32.8f)); range_dps = 1000.f;
_px4_gyro.set_range(math::radians(1000.f));
break; break;
case GYRO_FS_SEL_2000_DPS: case GYRO_FS_SEL_2000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 16.4f)); range_dps = 2000.f;
_px4_gyro.set_range(math::radians(2000.f));
break; break;
} }
_px4_gyro.set_scale(math::radians(range_dps / 32768.f));
_px4_gyro.set_range(math::radians(range_dps));
} }
void ICM40609D::ConfigureSampleRate(int sample_rate) void ICM40609D::ConfigureSampleRate(int sample_rate)

20
src/drivers/imu/invensense/icm42688p/ICM42688P.cpp
View File

@@ -279,32 +279,32 @@ void ICM42688P::ConfigureGyro()
{ {
const uint8_t GYRO_FS_SEL = RegisterRead(Register::BANK_0::GYRO_CONFIG0) & (Bit7 | Bit6 | Bit5); // 7:5 GYRO_FS_SEL const uint8_t GYRO_FS_SEL = RegisterRead(Register::BANK_0::GYRO_CONFIG0) & (Bit7 | Bit6 | Bit5); // 7:5 GYRO_FS_SEL
float range_dps = 0.f;
switch (GYRO_FS_SEL) { switch (GYRO_FS_SEL) {
case GYRO_FS_SEL_125_DPS: case GYRO_FS_SEL_125_DPS:
_px4_gyro.set_scale(math::radians(1.f / 262.f)); range_dps = 125.f;
_px4_gyro.set_range(math::radians(125.f));
break; break;
case GYRO_FS_SEL_250_DPS: case GYRO_FS_SEL_250_DPS:
_px4_gyro.set_scale(math::radians(1.f / 131.f)); range_dps = 250.f;
_px4_gyro.set_range(math::radians(250.f));
break; break;
case GYRO_FS_SEL_500_DPS: case GYRO_FS_SEL_500_DPS:
_px4_gyro.set_scale(math::radians(1.f / 65.5f)); range_dps = 500.f;
_px4_gyro.set_range(math::radians(500.f));
break; break;
case GYRO_FS_SEL_1000_DPS: case GYRO_FS_SEL_1000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 32.8f)); range_dps = 1000.f;
_px4_gyro.set_range(math::radians(1000.f));
break; break;
case GYRO_FS_SEL_2000_DPS: case GYRO_FS_SEL_2000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 16.4f)); range_dps = 2000.f;
_px4_gyro.set_range(math::radians(2000.f));
break; break;
} }
_px4_gyro.set_scale(math::radians(range_dps / 32768.f));
_px4_gyro.set_range(math::radians(range_dps));
} }
void ICM42688P::ConfigureSampleRate(int sample_rate) void ICM42688P::ConfigureSampleRate(int sample_rate)

17
src/drivers/imu/invensense/mpu6000/MPU6000.cpp
View File

@@ -309,27 +309,28 @@ void MPU6000::ConfigureGyro()
{ {
const uint8_t GYRO_FS_SEL = RegisterRead(Register::GYRO_CONFIG) & (Bit4 | Bit3); // [4:3] FS_SEL[1:0] const uint8_t GYRO_FS_SEL = RegisterRead(Register::GYRO_CONFIG) & (Bit4 | Bit3); // [4:3] FS_SEL[1:0]
float range_dps = 0.f;
switch (GYRO_FS_SEL) { switch (GYRO_FS_SEL) {
case FS_SEL_250_DPS: case FS_SEL_250_DPS:
_px4_gyro.set_scale(math::radians(1.f / 131.f)); range_dps = 250.f;
_px4_gyro.set_range(math::radians(250.f));
break; break;
case FS_SEL_500_DPS: case FS_SEL_500_DPS:
_px4_gyro.set_scale(math::radians(1.f / 65.5f)); range_dps = 500.f;
_px4_gyro.set_range(math::radians(500.f));
break; break;
case FS_SEL_1000_DPS: case FS_SEL_1000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 32.8f)); range_dps = 1000.f;
_px4_gyro.set_range(math::radians(1000.f));
break; break;
case FS_SEL_2000_DPS: case FS_SEL_2000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 16.4f)); range_dps = 2000.f;
_px4_gyro.set_range(math::radians(2000.f));
break; break;
} }
_px4_gyro.set_scale(math::radians(range_dps / 32768.f));
_px4_gyro.set_range(math::radians(range_dps));
} }
void MPU6000::ConfigureSampleRate(int sample_rate) void MPU6000::ConfigureSampleRate(int sample_rate)

17
src/drivers/imu/invensense/mpu6500/MPU6500.cpp
View File

@@ -284,27 +284,28 @@ void MPU6500::ConfigureGyro()
{ {
const uint8_t GYRO_FS_SEL = RegisterRead(Register::GYRO_CONFIG) & (Bit4 | Bit3); // [4:3] GYRO_FS_SEL[1:0] const uint8_t GYRO_FS_SEL = RegisterRead(Register::GYRO_CONFIG) & (Bit4 | Bit3); // [4:3] GYRO_FS_SEL[1:0]
float range_dps = 0.f;
switch (GYRO_FS_SEL) { switch (GYRO_FS_SEL) {
case GYRO_FS_SEL_250_DPS: case GYRO_FS_SEL_250_DPS:
_px4_gyro.set_scale(math::radians(1.f / 131.f)); range_dps = 250.f;
_px4_gyro.set_range(math::radians(250.f));
break; break;
case GYRO_FS_SEL_500_DPS: case GYRO_FS_SEL_500_DPS:
_px4_gyro.set_scale(math::radians(1.f / 65.5f)); range_dps = 500.f;
_px4_gyro.set_range(math::radians(500.f));
break; break;
case GYRO_FS_SEL_1000_DPS: case GYRO_FS_SEL_1000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 32.8f)); range_dps = 1000.f;
_px4_gyro.set_range(math::radians(1000.f));
break; break;
case GYRO_FS_SEL_2000_DPS: case GYRO_FS_SEL_2000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 16.4f)); range_dps = 2000.f;
_px4_gyro.set_range(math::radians(2000.f));
break; break;
} }
_px4_gyro.set_scale(math::radians(range_dps / 32768.f));
_px4_gyro.set_range(math::radians(range_dps));
} }
void MPU6500::ConfigureSampleRate(int sample_rate) void MPU6500::ConfigureSampleRate(int sample_rate)

17
src/drivers/imu/invensense/mpu9250/MPU9250.cpp
View File

@@ -317,27 +317,28 @@ void MPU9250::ConfigureGyro()
{ {
const uint8_t GYRO_FS_SEL = RegisterRead(Register::GYRO_CONFIG) & (Bit4 | Bit3); // [4:3] GYRO_FS_SEL[1:0] const uint8_t GYRO_FS_SEL = RegisterRead(Register::GYRO_CONFIG) & (Bit4 | Bit3); // [4:3] GYRO_FS_SEL[1:0]
float range_dps = 0.f;
switch (GYRO_FS_SEL) { switch (GYRO_FS_SEL) {
case GYRO_FS_SEL_250_DPS: case GYRO_FS_SEL_250_DPS:
_px4_gyro.set_scale(math::radians(1.f / 131.f)); range_dps = 250.f;
_px4_gyro.set_range(math::radians(250.f));
break; break;
case GYRO_FS_SEL_500_DPS: case GYRO_FS_SEL_500_DPS:
_px4_gyro.set_scale(math::radians(1.f / 65.5f)); range_dps = 500.f;
_px4_gyro.set_range(math::radians(500.f));
break; break;
case GYRO_FS_SEL_1000_DPS: case GYRO_FS_SEL_1000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 32.8f)); range_dps = 1000.f;
_px4_gyro.set_range(math::radians(1000.f));
break; break;
case GYRO_FS_SEL_2000_DPS: case GYRO_FS_SEL_2000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 16.4f)); range_dps = 2000.f;
_px4_gyro.set_range(math::radians(2000.f));
break; break;
} }
_px4_gyro.set_scale(math::radians(range_dps / 32768.f));
_px4_gyro.set_range(math::radians(range_dps));
} }
void MPU9250::ConfigureSampleRate(int sample_rate) void MPU9250::ConfigureSampleRate(int sample_rate)