Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

AP_InertialSensor: Rework rotation handling #5216

Closed
wants to merge 1 commit into from
Closed

AP_InertialSensor: Rework rotation handling #5216

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
22 changes: 12 additions & 10 deletions libraries/AP_HAL/board/linux.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,9 +18,9 @@
#define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain"
#define HAL_INS_DEFAULT HAL_INS_MPU9250_SPI
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF
#define HAL_INS_DEFAULT_ROTATION ROTATION_ROLL_180_YAW_270
#define HAL_INS_DEFAULT_ROTATION ROTATION_YAW_180
#else
#define HAL_INS_DEFAULT_ROTATION ROTATION_ROLL_180_YAW_90
#define HAL_INS_DEFAULT_ROTATION ROTATION_NONE
#endif
#define HAL_INS_MPU9250_NAME "mpu9250"
#define HAL_BARO_DEFAULT HAL_BARO_MS5611_SPI
Expand All @@ -37,7 +37,7 @@
#define HAL_BOARD_LOG_DIRECTORY "/data/ftp/internal_000/ardupilot/logs"
#define HAL_BOARD_TERRAIN_DIRECTORY "/data/ftp/internal_000/ardupilot/terrain"
#define HAL_INS_DEFAULT HAL_INS_MPU60XX_I2C
#define HAL_INS_DEFAULT_ROTATION ROTATION_YAW_270
#define HAL_INS_DEFAULT_ROTATION ROTATION_ROLL_180
#define HAL_INS_MPU60x0_I2C_BUS 2
#define HAL_INS_MPU60x0_I2C_ADDR 0x68
#define HAL_COMPASS_DEFAULT HAL_COMPASS_AK8963_I2C
Expand Down Expand Up @@ -79,7 +79,7 @@
#define HAL_BOARD_LOG_DIRECTORY "/data/ftp/internal_000/ardupilot/logs"
#define HAL_BOARD_TERRAIN_DIRECTORY "/data/ftp/internal_000/ardupilot/terrain"
#define HAL_INS_DEFAULT HAL_INS_MPU60XX_I2C
#define HAL_INS_DEFAULT_ROTATION ROTATION_PITCH_180_YAW_90
#define HAL_INS_DEFAULT_ROTATION ROTATION_YAW_180
#define HAL_INS_MPU60x0_I2C_BUS 2
#define HAL_INS_MPU60x0_I2C_ADDR 0x68
#define HAL_COMPASS_DEFAULT HAL_COMPASS_AK8963_I2C
Expand Down Expand Up @@ -122,7 +122,7 @@
#define HAL_BOARD_LOG_DIRECTORY "/var/APM/logs"
#define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain"
#define HAL_INS_DEFAULT HAL_INS_MPU60XX_SPI
#define HAL_INS_DEFAULT_ROTATION ROTATION_YAW_90
#define HAL_INS_DEFAULT_ROTATION ROTATION_PITCH_180
#define HAL_INS_MPU60x0_NAME "mpu6000"
#define HAL_BARO_DEFAULT HAL_BARO_MS5611_I2C
#define HAL_BARO_MS5611_I2C_BUS 10
Expand All @@ -148,7 +148,7 @@
#define HAL_BOARD_LOG_DIRECTORY "/var/APM/logs"
#define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain"
#define HAL_INS_DEFAULT HAL_INS_MPU9250_SPI
#define HAL_INS_DEFAULT_ROTATION ROTATION_NONE
#define HAL_INS_DEFAULT_ROTATION ROTATION_ROLL_180_YAW_90
#define HAL_INS_MPU9250_NAME "mpu9250"
#define HAL_BARO_DEFAULT HAL_BARO_MS5611_I2C
#define HAL_BARO_MS5611_I2C_BUS 1
Expand All @@ -161,7 +161,7 @@
#define HAL_BOARD_LOG_DIRECTORY "/var/APM/logs"
#define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain"
#define HAL_INS_DEFAULT HAL_INS_MPU9250_SPI
#define HAL_INS_DEFAULT_ROTATION ROTATION_NONE
#define HAL_INS_DEFAULT_ROTATION ROTATION_ROLL_180_YAW_90
#define HAL_INS_MPU9250_NAME "mpu9250"
#define HAL_BARO_DEFAULT HAL_BARO_MS5611_I2C
#define HAL_BARO_MS5611_I2C_BUS 1
Expand All @@ -178,6 +178,7 @@
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
#define HAL_BOARD_LOG_DIRECTORY "/var/APM/logs"
#define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain"
#define HAL_INS_DEFAULT_ROTATION ROTATION_ROLL_180_YAW_90
#define HAL_INS_DEFAULT HAL_INS_RASPILOT
#define HAL_INS_MPU60x0_NAME "mpu6000"
#define HAL_INS_LSM9DS0_G_NAME "lsm9ds0_g"
Expand Down Expand Up @@ -221,7 +222,7 @@
#define HAL_GPIO_LED_ON LOW
#define HAL_GPIO_LED_OFF HIGH
#define HAL_INS_DEFAULT HAL_INS_BBBMINI
#define HAL_INS_DEFAULT_ROTATION ROTATION_NONE
#define HAL_INS_DEFAULT_ROTATION ROTATION_ROLL_180_YAW_90
#define HAL_INS_MPU9250_NAME "mpu9250"
#define HAL_INS_MPU9250_NAME_EXT "mpu9250ext"
#define HAL_BARO_DEFAULT HAL_BARO_MS5611_SPI
Expand Down Expand Up @@ -262,6 +263,7 @@
#define HAL_BARO_MS5611_I2C_BUS 1
#define HAL_BARO_MS5611_I2C_ADDR 0x77
#define HAL_INS_DEFAULT HAL_INS_BH
#define HAL_INS_DEFAULT_ROTATION ROTATION_ROLL_180_YAW_90
#define HAL_INS_MPU9250_NAME "mpu9250"
#define HAL_INS_MPU60x0_I2C_BUS 1
#define HAL_INS_MPU60x0_I2C_ADDR 0x69
Expand All @@ -288,7 +290,7 @@
#define HAL_BOARD_LOG_DIRECTORY "/var/APM/logs"
#define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain"
#define HAL_INS_DEFAULT HAL_INS_MPU9250_SPI
#define HAL_INS_DEFAULT_ROTATION ROTATION_YAW_270
#define HAL_INS_DEFAULT_ROTATION ROTATION_ROLL_180
#define HAL_INS_MPU9250_NAME "mpu9250"
#define HAL_BARO_DEFAULT HAL_BARO_MS5611_SPI
#define HAL_BARO_MS5611_NAME "ms5611"
Expand All @@ -315,7 +317,7 @@
#define HAL_BOARD_LOG_DIRECTORY "/var/APM/logs"
#define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain"
#define HAL_INS_DEFAULT HAL_INS_MPU9250_SPI
#define HAL_INS_DEFAULT_ROTATION ROTATION_NONE
#define HAL_INS_DEFAULT_ROTATION ROTATION_ROLL_180_YAW_90
#define HAL_INS_MPU9250_NAME "mpu9250"
#define HAL_COMPASS_DEFAULT HAL_COMPASS_AK8963_MPU9250
#define HAL_BARO_DEFAULT HAL_BARO_MS5611_I2C
Expand Down
40 changes: 21 additions & 19 deletions libraries/AP_InertialSensor/AP_InertialSensor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -694,11 +694,11 @@ AP_InertialSensor::detect_backends(void)

switch (AP_BoardConfig::get_board_type()) {
case AP_BoardConfig::PX4_BOARD_PX4V1:
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME)));
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_ROLL_180_YAW_90));
break;

case AP_BoardConfig::PX4_BOARD_PIXHAWK:
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_ROLL_180));
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_YAW_270));
_add_backend(AP_InertialSensor_LSM9DS0::probe(*this,
hal.spi->get_device(HAL_INS_LSM9DS0_G_NAME),
hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME),
Expand All @@ -710,44 +710,44 @@ AP_InertialSensor::detect_backends(void)
case AP_BoardConfig::PX4_BOARD_PIXHAWK2:
// older Pixhawk2 boards have the MPU6000 instead of MPU9250
_fast_sampling_mask.set_default(1);
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_EXT_NAME), ROTATION_PITCH_180));
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_EXT_NAME), ROTATION_YAW_90));
_add_backend(AP_InertialSensor_LSM9DS0::probe(*this,
hal.spi->get_device(HAL_INS_LSM9DS0_EXT_G_NAME),
hal.spi->get_device(HAL_INS_LSM9DS0_EXT_A_NAME),
ROTATION_ROLL_180_YAW_270,
ROTATION_ROLL_180_YAW_90,
ROTATION_ROLL_180_YAW_90));
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_YAW_270));
ROTATION_YAW_270,
ROTATION_YAW_90,
ROTATION_YAW_90));
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_ROLL_180));
break;

case AP_BoardConfig::PX4_BOARD_PIXRACER:
_fast_sampling_mask.set_default(3);
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_ICM20608_NAME), ROTATION_ROLL_180_YAW_90));
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_ROLL_180_YAW_90));
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_ICM20608_NAME), ROTATION_NONE));
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_NONE));
break;

case AP_BoardConfig::PX4_BOARD_PHMINI:
// PHMINI uses ICM20608 on the ACCEL_MAG device and a MPU9250 on the old MPU6000 CS line
_fast_sampling_mask.set_default(3);
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_ICM20608_AM_NAME), ROTATION_ROLL_180));
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_ROLL_180));
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_ICM20608_AM_NAME), ROTATION_YAW_270));
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_YAW_270));
break;

case AP_BoardConfig::PX4_BOARD_AUAV21:
// AUAV2.1 uses ICM20608 on the ACCEL_MAG device and a MPU9250 on the old MPU6000 CS line
_fast_sampling_mask.set_default(3);
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_ICM20608_AM_NAME), ROTATION_ROLL_180_YAW_90));
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_ROLL_180_YAW_90));
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_ICM20608_AM_NAME), ROTATION_NONE));
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_NONE));
break;

case AP_BoardConfig::PX4_BOARD_PH2SLIM:
_fast_sampling_mask.set_default(1);
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_YAW_270));
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_ROLL_180));
break;

case AP_BoardConfig::PX4_BOARD_AEROFC:
_fast_sampling_mask.set_default(1);
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU6500_NAME), ROTATION_YAW_270));
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU6500_NAME), ROTATION_ROLL_180));
break;

default:
Expand All @@ -758,19 +758,21 @@ AP_InertialSensor::detect_backends(void)
#elif HAL_INS_DEFAULT == HAL_INS_MPU9250_SPI && defined(HAL_INS_DEFAULT_ROTATION)
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), HAL_INS_DEFAULT_ROTATION));
#elif HAL_INS_DEFAULT == HAL_INS_MPU9250_SPI
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME)));
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), HAL_INS_DEFAULT_ROTATION));
#elif HAL_INS_DEFAULT == HAL_INS_LSM9DS0
_add_backend(AP_InertialSensor_LSM9DS0::probe(*this,
hal.spi->get_device(HAL_INS_LSM9DS0_G_NAME),
hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME)));
#elif HAL_INS_DEFAULT == HAL_INS_L3G4200D && defined(HAL_INS_DEFAULT_ROTATION)
_add_backend(AP_InertialSensor_L3G4200D::probe(*this, hal.i2c_mgr->get_device(HAL_INS_L3G4200D_I2C_BUS, HAL_INS_L3G4200D_I2C_ADDR), HAL_INS_DEFAULT_ROTATION));
#elif HAL_INS_DEFAULT == HAL_INS_L3G4200D
_add_backend(AP_InertialSensor_L3G4200D::probe(*this, hal.i2c_mgr->get_device(HAL_INS_L3G4200D_I2C_BUS, HAL_INS_L3G4200D_I2C_ADDR)));
#elif HAL_INS_DEFAULT == HAL_INS_RASPILOT
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME)));
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), HAL_INS_DEFAULT_ROTATION));
_add_backend(AP_InertialSensor_LSM9DS0::probe(*this,
hal.spi->get_device(HAL_INS_LSM9DS0_G_NAME),
hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME),
ROTATION_NONE, ROTATION_YAW_90));
ROTATION_ROLL_180, ROTATION_ROLL_180_YAW_90));
#elif HAL_INS_DEFAULT == HAL_INS_MPU9250_I2C
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU9250_I2C_BUS, HAL_INS_MPU9250_I2C_ADDR)));
#elif HAL_INS_DEFAULT == HAL_INS_QFLIGHT
Expand Down
24 changes: 12 additions & 12 deletions libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -569,9 +569,9 @@ bool AP_InertialSensor_Invensense::_accumulate(uint8_t *samples, uint8_t n_sampl
fsync_set = (int16_val(data, 2) & 1U) != 0;
#endif

accel = Vector3f(int16_val(data, 1),
int16_val(data, 0),
-int16_val(data, 2));
accel = Vector3f(int16_val(data, 0),
int16_val(data, 1),
int16_val(data, 2));
accel *= _accel_scale;

int16_t t2 = int16_val(data, 3);
Expand All @@ -582,9 +582,9 @@ bool AP_InertialSensor_Invensense::_accumulate(uint8_t *samples, uint8_t n_sampl
}
float temp = t2 * temp_sensitivity + temp_zero;

gyro = Vector3f(int16_val(data, 5),
int16_val(data, 4),
-int16_val(data, 6));
gyro = Vector3f(int16_val(data, 4),
int16_val(data, 5),
int16_val(data, 6));
gyro *= GYRO_SCALE;

_rotate_and_correct_accel(_accel_instance, accel);
Expand Down Expand Up @@ -628,9 +628,9 @@ bool AP_InertialSensor_Invensense::_accumulate_fast_sampling(uint8_t *samples, u

if ((_accum.count & 1) == 0) {
// accel data is at 4kHz
Vector3f a(int16_val(data, 1),
int16_val(data, 0),
-int16_val(data, 2));
Vector3f a(int16_val(data, 0),
int16_val(data, 1),
int16_val(data, 2));
if (fabsf(a.x) > clip_limit ||
fabsf(a.y) > clip_limit ||
fabsf(a.z) > clip_limit) {
Expand All @@ -639,9 +639,9 @@ bool AP_InertialSensor_Invensense::_accumulate_fast_sampling(uint8_t *samples, u
_accum.accel += _accum.accel_filter.apply(a);
}

Vector3f g(int16_val(data, 5),
int16_val(data, 4),
-int16_val(data, 6));
Vector3f g(int16_val(data, 4),
int16_val(data, 5),
int16_val(data, 6));

_accum.gyro += _accum.gyro_filter.apply(g);
_accum.count++;
Expand Down
4 changes: 2 additions & 2 deletions libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -251,7 +251,7 @@ void AP_InertialSensor_L3G4200D::_accumulate(void)
if (!_dev->read_registers(L3G4200D_REG_XL | L3G4200D_REG_AUTO_INCREMENT,
(uint8_t *)&buffer, sizeof(buffer))) {
for (uint8_t i=0; i < num_samples_available; i++) {
Vector3f gyro = Vector3f(buffer[i][0], -buffer[i][1], -buffer[i][2]);
Vector3f gyro = Vector3f(buffer[i][0], buffer[i][1], buffer[i][2]);
// Adjust for chip scaling to get radians/sec
gyro *= L3G4200D_GYRO_SCALE_R_S;
_rotate_and_correct_gyro(_gyro_instance, gyro);
Expand All @@ -272,7 +272,7 @@ void AP_InertialSensor_L3G4200D::_accumulate(void)
(uint8_t *)buffer, sizeof(buffer[0]),
num_samples_available)) {
for (uint8_t i=0; i<num_samples_available; i++) {
Vector3f accel = Vector3f(buffer[i][0], -buffer[i][1], -buffer[i][2]);
Vector3f accel = Vector3f(buffer[i][0], buffer[i][1], buffer[i][2]);
// Adjust for chip scaling to get m/s/s
accel *= ADXL345_ACCELEROMETER_SCALE_M_S;
_rotate_and_correct_accel(_accel_instance, accel);
Expand Down
4 changes: 2 additions & 2 deletions libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -740,7 +740,7 @@ void AP_InertialSensor_LSM9DS0::_read_data_transaction_a()
return;
}

Vector3f accel_data(raw_data.x, -raw_data.y, -raw_data.z);
Vector3f accel_data(raw_data.x, raw_data.y, raw_data.z);
accel_data *= _accel_scale;

_rotate_and_correct_accel(_accel_instance, accel_data);
Expand All @@ -760,7 +760,7 @@ void AP_InertialSensor_LSM9DS0::_read_data_transaction_g()
return;
}

Vector3f gyro_data(raw_data.x, -raw_data.y, -raw_data.z);
Vector3f gyro_data(raw_data.x, raw_data.y, raw_data.z);

gyro_data *= _gyro_scale;

Expand Down