zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

drivers/imu/invensense: simplify gyro range configuration

sbg
Daniel Agar 5 years ago
parent
1efc8c27eb
commit
daf75e40eb
10 changed files with 92 additions and 84 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 17
      src/drivers/imu/invensense/icm20602/ICM20602.cpp
  2. 17
      src/drivers/imu/invensense/icm20608g/ICM20608G.cpp
  3. 17
      src/drivers/imu/invensense/icm20649/ICM20649.cpp
  4. 17
      src/drivers/imu/invensense/icm20689/ICM20689.cpp
  5. 17
      src/drivers/imu/invensense/icm20948/ICM20948.cpp
  6. 20
      src/drivers/imu/invensense/icm40609d/ICM40609D.cpp
  7. 20
      src/drivers/imu/invensense/icm42688p/ICM42688P.cpp
  8. 17
      src/drivers/imu/invensense/mpu6000/MPU6000.cpp
  9. 17
      src/drivers/imu/invensense/mpu6500/MPU6500.cpp
  10. 17
      src/drivers/imu/invensense/mpu9250/MPU9250.cpp

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

@ -276,27 +276,28 @@ void ICM20602::ConfigureGyro() @@ -276,27 +276,28 @@ void ICM20602::ConfigureGyro()
{
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) {
case FS_SEL_250_DPS:
_px4_gyro.set_scale(math::radians(1.f / 131.f));
_px4_gyro.set_range(math::radians(250.f));
range_dps = 250.f;
break;
case FS_SEL_500_DPS:
_px4_gyro.set_scale(math::radians(1.f / 65.5f));
_px4_gyro.set_range(math::radians(500.f));
range_dps = 500.f;
break;
case FS_SEL_1000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 32.8f));
_px4_gyro.set_range(math::radians(1000.f));
range_dps = 1000.f;
break;
case FS_SEL_2000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 16.4f));
_px4_gyro.set_range(math::radians(2000.f));
range_dps = 2000.f;
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)

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

@ -309,27 +309,28 @@ void ICM20608G::ConfigureGyro() @@ -309,27 +309,28 @@ void ICM20608G::ConfigureGyro()
{
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) {
case FS_SEL_250_DPS:
_px4_gyro.set_scale(math::radians(1.f / 131.f));
_px4_gyro.set_range(math::radians(250.f));
range_dps = 250.f;
break;
case FS_SEL_500_DPS:
_px4_gyro.set_scale(math::radians(1.f / 65.5f));
_px4_gyro.set_range(math::radians(500.f));
range_dps = 500.f;
break;
case FS_SEL_1000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 32.8f));
_px4_gyro.set_range(math::radians(1000.f));
range_dps = 1000.f;
break;
case FS_SEL_2000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 16.4f));
_px4_gyro.set_range(math::radians(2000.f));
range_dps = 2000.f;
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)

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

@ -291,27 +291,28 @@ void ICM20649::ConfigureGyro() @@ -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]
float range_dps = 0.f;
switch (GYRO_FS_SEL) {
case GYRO_FS_SEL_500_DPS:
_px4_gyro.set_scale(math::radians(1.f / 65.5f));
_px4_gyro.set_range(math::radians(500.f));
range_dps = 500.f;
break;
case GYRO_FS_SEL_1000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 32.8f));
_px4_gyro.set_range(math::radians(1000.f));
range_dps = 1000.f;
break;
case GYRO_FS_SEL_2000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 16.4f));
_px4_gyro.set_range(math::radians(2000.f));
range_dps = 2000.f;
break;
case GYRO_FS_SEL_4000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 8.2f));
_px4_gyro.set_range(math::radians(4000.f));
range_dps = 4000.f;
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)

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

@ -309,27 +309,28 @@ void ICM20689::ConfigureGyro() @@ -309,27 +309,28 @@ void ICM20689::ConfigureGyro()
{
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) {
case FS_SEL_250_DPS:
_px4_gyro.set_scale(math::radians(1.f / 131.f));
_px4_gyro.set_range(math::radians(250.f));
range_dps = 250.f;
break;
case FS_SEL_500_DPS:
_px4_gyro.set_scale(math::radians(1.f / 65.5f));
_px4_gyro.set_range(math::radians(500.f));
range_dps = 500.f;
break;
case FS_SEL_1000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 32.8f));
_px4_gyro.set_range(math::radians(1000.f));
range_dps = 1000.f;
break;
case FS_SEL_2000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 16.4f));
_px4_gyro.set_range(math::radians(2000.f));
range_dps = 2000.f;
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)

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

@ -325,27 +325,28 @@ void ICM20948::ConfigureGyro() @@ -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]
float range_dps = 0.f;
switch (GYRO_FS_SEL) {
case GYRO_FS_SEL_250_DPS:
_px4_gyro.set_scale(math::radians(1.f / 131.f));
_px4_gyro.set_range(math::radians(250.f));
range_dps = 250.f;
break;
case GYRO_FS_SEL_500_DPS:
_px4_gyro.set_scale(math::radians(1.f / 65.5f));
_px4_gyro.set_range(math::radians(500.f));
range_dps = 500.f;
break;
case GYRO_FS_SEL_1000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 32.8f));
_px4_gyro.set_range(math::radians(1000.f));
range_dps = 1000.f;
break;
case GYRO_FS_SEL_2000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 16.4f));
_px4_gyro.set_range(math::radians(2000.f));
range_dps = 2000.f;
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)

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

@ -277,32 +277,32 @@ void ICM40609D::ConfigureGyro() @@ -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
float range_dps = 0.f;
switch (GYRO_FS_SEL) {
case GYRO_FS_SEL_125_DPS:
_px4_gyro.set_scale(math::radians(1.f / 262.f));
_px4_gyro.set_range(math::radians(125.f));
range_dps = 125.f;
break;
case GYRO_FS_SEL_250_DPS:
_px4_gyro.set_scale(math::radians(1.f / 131.f));
_px4_gyro.set_range(math::radians(250.f));
range_dps = 250.f;
break;
case GYRO_FS_SEL_500_DPS:
_px4_gyro.set_scale(math::radians(1.f / 65.5f));
_px4_gyro.set_range(math::radians(500.f));
range_dps = 500.f;
break;
case GYRO_FS_SEL_1000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 32.8f));
_px4_gyro.set_range(math::radians(1000.f));
range_dps = 1000.f;
break;
case GYRO_FS_SEL_2000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 16.4f));
_px4_gyro.set_range(math::radians(2000.f));
range_dps = 2000.f;
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)

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

@ -279,32 +279,32 @@ void ICM42688P::ConfigureGyro() @@ -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
float range_dps = 0.f;
switch (GYRO_FS_SEL) {
case GYRO_FS_SEL_125_DPS:
_px4_gyro.set_scale(math::radians(1.f / 262.f));
_px4_gyro.set_range(math::radians(125.f));
range_dps = 125.f;
break;
case GYRO_FS_SEL_250_DPS:
_px4_gyro.set_scale(math::radians(1.f / 131.f));
_px4_gyro.set_range(math::radians(250.f));
range_dps = 250.f;
break;
case GYRO_FS_SEL_500_DPS:
_px4_gyro.set_scale(math::radians(1.f / 65.5f));
_px4_gyro.set_range(math::radians(500.f));
range_dps = 500.f;
break;
case GYRO_FS_SEL_1000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 32.8f));
_px4_gyro.set_range(math::radians(1000.f));
range_dps = 1000.f;
break;
case GYRO_FS_SEL_2000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 16.4f));
_px4_gyro.set_range(math::radians(2000.f));
range_dps = 2000.f;
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)

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

@ -309,27 +309,28 @@ void MPU6000::ConfigureGyro() @@ -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]
float range_dps = 0.f;
switch (GYRO_FS_SEL) {
case FS_SEL_250_DPS:
_px4_gyro.set_scale(math::radians(1.f / 131.f));
_px4_gyro.set_range(math::radians(250.f));
range_dps = 250.f;
break;
case FS_SEL_500_DPS:
_px4_gyro.set_scale(math::radians(1.f / 65.5f));
_px4_gyro.set_range(math::radians(500.f));
range_dps = 500.f;
break;
case FS_SEL_1000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 32.8f));
_px4_gyro.set_range(math::radians(1000.f));
range_dps = 1000.f;
break;
case FS_SEL_2000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 16.4f));
_px4_gyro.set_range(math::radians(2000.f));
range_dps = 2000.f;
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)

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

@ -284,27 +284,28 @@ void MPU6500::ConfigureGyro() @@ -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]
float range_dps = 0.f;
switch (GYRO_FS_SEL) {
case GYRO_FS_SEL_250_DPS:
_px4_gyro.set_scale(math::radians(1.f / 131.f));
_px4_gyro.set_range(math::radians(250.f));
range_dps = 250.f;
break;
case GYRO_FS_SEL_500_DPS:
_px4_gyro.set_scale(math::radians(1.f / 65.5f));
_px4_gyro.set_range(math::radians(500.f));
range_dps = 500.f;
break;
case GYRO_FS_SEL_1000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 32.8f));
_px4_gyro.set_range(math::radians(1000.f));
range_dps = 1000.f;
break;
case GYRO_FS_SEL_2000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 16.4f));
_px4_gyro.set_range(math::radians(2000.f));
range_dps = 2000.f;
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)

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

@ -317,27 +317,28 @@ void MPU9250::ConfigureGyro() @@ -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]
float range_dps = 0.f;
switch (GYRO_FS_SEL) {
case GYRO_FS_SEL_250_DPS:
_px4_gyro.set_scale(math::radians(1.f / 131.f));
_px4_gyro.set_range(math::radians(250.f));
range_dps = 250.f;
break;
case GYRO_FS_SEL_500_DPS:
_px4_gyro.set_scale(math::radians(1.f / 65.5f));
_px4_gyro.set_range(math::radians(500.f));
range_dps = 500.f;
break;
case GYRO_FS_SEL_1000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 32.8f));
_px4_gyro.set_range(math::radians(1000.f));
range_dps = 1000.f;
break;
case GYRO_FS_SEL_2000_DPS:
_px4_gyro.set_scale(math::radians(1.f / 16.4f));
_px4_gyro.set_range(math::radians(2000.f));
range_dps = 2000.f;
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)

Write Preview
Loading…
Cancel
Save