Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 01:33:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: support the L3GD20H gyro

Browse Source 
this is used by the MRo Pixhawk1
This commit is contained in:
Andrew Tridgell 2017-01-30 14:49:03 +11:00
parent 9abb0bba7f
commit 01da36295d
3 changed files with 32 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -699,7 +699,9 @@ AP_InertialSensor::detect_backends(void)
_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_ROLL_180, ROTATION_ROLL_180_YAW_270));
ROTATION_ROLL_180,
ROTATION_ROLL_180_YAW_270,
ROTATION_PITCH_180));
break;
case AP_BoardConfig::PX4_BOARD_PIXHAWK2:
@ -709,7 +711,9 @@ AP_InertialSensor::detect_backends(void)
_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_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));
break;

25
libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp
View File

@ -31,7 +31,8 @@ extern const AP_HAL::HAL &hal;
#define LSM9DS0_DRY_G_PIN -1
#endif
#define LSM9DS0_G_WHOAMI 0xD4
#define LSM9DS0_G_WHOAMI 0xD4 // L3GD20
#define LSM9DS0_G_WHOAMI_H 0xD7 // L3GD20H
#define LSM9DS0_XM_WHOAMI 0x49
////////////////////////////
@ -383,7 +384,8 @@ AP_InertialSensor_LSM9DS0::AP_InertialSensor_LSM9DS0(AP_InertialSensor &imu,
int drdy_pin_num_a,
int drdy_pin_num_g,
enum Rotation rotation_a,
enum Rotation rotation_g)
enum Rotation rotation_g,
enum Rotation rotation_gH)
: AP_InertialSensor_Backend(imu)
, _dev_gyro(std::move(dev_gyro))
, _dev_accel(std::move(dev_accel))
@ -391,6 +393,7 @@ AP_InertialSensor_LSM9DS0::AP_InertialSensor_LSM9DS0(AP_InertialSensor &imu,
, _drdy_pin_num_g(drdy_pin_num_g)
, _rotation_a(rotation_a)
, _rotation_g(rotation_g)
, _rotation_gH(rotation_gH)
{
}
@ -398,7 +401,8 @@ AP_InertialSensor_Backend *AP_InertialSensor_LSM9DS0::probe(AP_InertialSensor &_
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev_gyro,
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev_accel,
enum Rotation rotation_a,
enum Rotation rotation_g)
enum Rotation rotation_g,
enum Rotation rotation_gH)
{
if (!dev_gyro || !dev_accel) {
return nullptr;
@ -406,7 +410,7 @@ AP_InertialSensor_Backend *AP_InertialSensor_LSM9DS0::probe(AP_InertialSensor &_
AP_InertialSensor_LSM9DS0 *sensor =
new AP_InertialSensor_LSM9DS0(_imu, std::move(dev_gyro), std::move(dev_accel),
LSM9DS0_DRY_X_PIN, LSM9DS0_DRY_G_PIN,
rotation_a, rotation_g);
rotation_a, rotation_g, rotation_gH);
if (!sensor || !sensor->_init_sensor()) {
delete sensor;
return nullptr;
@ -456,16 +460,15 @@ bool AP_InertialSensor_LSM9DS0::_hardware_init()
return false;
}
uint8_t whoami;
uint8_t tries;
uint8_t tries, whoami;
// set flag for reading registers
_dev_gyro->set_read_flag(0x80);
_dev_accel->set_read_flag(0x80);
whoami = _register_read_g(WHO_AM_I_G);
if (whoami != LSM9DS0_G_WHOAMI) {
hal.console->printf("LSM9DS0: unexpected gyro WHOAMI 0x%x\n", (unsigned)whoami);
whoami_g = _register_read_g(WHO_AM_I_G);
if (whoami_g != LSM9DS0_G_WHOAMI && whoami_g != LSM9DS0_G_WHOAMI_H) {
hal.console->printf("LSM9DS0: unexpected gyro WHOAMI 0x%x\n", (unsigned)whoami_g);
goto fail_whoami;
}
@ -522,7 +525,11 @@ void AP_InertialSensor_LSM9DS0::start(void)
_gyro_instance = _imu.register_gyro(760, _dev_gyro->get_bus_id_devtype(DEVTYPE_GYR_L3GD20));
_accel_instance = _imu.register_accel(800, _dev_accel->get_bus_id_devtype(DEVTYPE_ACC_LSM303D));
if (whoami_g == LSM9DS0_G_WHOAMI_H) {
set_gyro_orientation(_gyro_instance, _rotation_gH);
} else {
set_gyro_orientation(_gyro_instance, _rotation_g);
}
set_accel_orientation(_accel_instance, _rotation_a);
_set_accel_max_abs_offset(_accel_instance, 5.0f);

12
libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h
View File

@ -19,7 +19,8 @@ public:
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev_gyro,
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev_accel,
enum Rotation rotation_a = ROTATION_NONE,
enum Rotation rotation_g = ROTATION_NONE);
enum Rotation rotation_g = ROTATION_NONE,
enum Rotation rotation_gH = ROTATION_NONE);
private:
AP_InertialSensor_LSM9DS0(AP_InertialSensor &imu,
@ -27,7 +28,8 @@ private:
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev_accel,
int drdy_pin_num_a, int drdy_pin_num_b,
enum Rotation rotation_a,
enum Rotation rotation_g);
enum Rotation rotation_g,
enum Rotation rotation_gH);
struct PACKED sensor_raw_data {
int16_t x;
@ -97,10 +99,14 @@ private:
uint8_t _gyro_instance;
uint8_t _accel_instance;
// gyro whoami
uint8_t whoami_g;
/*
for boards that have a separate LSM303D and L3GD20 there can be
different rotations for each
*/
enum Rotation _rotation_a;
enum Rotation _rotation_g;
enum Rotation _rotation_g; // for L3GD20
enum Rotation _rotation_gH; // for L3GD20H
};