Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 09:43:57 -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, _add_backend(AP_InertialSensor_LSM9DS0::probe(*this,
hal.spi->get_device(HAL_INS_LSM9DS0_G_NAME), hal.spi->get_device(HAL_INS_LSM9DS0_G_NAME),
hal.spi->get_device(HAL_INS_LSM9DS0_A_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; break;
case AP_BoardConfig::PX4_BOARD_PIXHAWK2: case AP_BoardConfig::PX4_BOARD_PIXHAWK2:
@ -709,7 +711,9 @@ AP_InertialSensor::detect_backends(void)
_add_backend(AP_InertialSensor_LSM9DS0::probe(*this, _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_G_NAME),
hal.spi->get_device(HAL_INS_LSM9DS0_EXT_A_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)); _add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_YAW_270));
break; break;

27
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 #define LSM9DS0_DRY_G_PIN -1
#endif #endif
#define LSM9DS0_G_WHOAMI 0xD4 #define LSM9DS0_G_WHOAMI 0xD4 // L3GD20
#define LSM9DS0_G_WHOAMI_H 0xD7 // L3GD20H
#define LSM9DS0_XM_WHOAMI 0x49 #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_a,
int drdy_pin_num_g, int drdy_pin_num_g,
enum Rotation rotation_a, enum Rotation rotation_a,
enum Rotation rotation_g) enum Rotation rotation_g,
enum Rotation rotation_gH)
: AP_InertialSensor_Backend(imu) : AP_InertialSensor_Backend(imu)
, _dev_gyro(std::move(dev_gyro)) , _dev_gyro(std::move(dev_gyro))
, _dev_accel(std::move(dev_accel)) , _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) , _drdy_pin_num_g(drdy_pin_num_g)
, _rotation_a(rotation_a) , _rotation_a(rotation_a)
, _rotation_g(rotation_g) , _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_gyro,
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev_accel, AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev_accel,
enum Rotation rotation_a, enum Rotation rotation_a,
enum Rotation rotation_g) enum Rotation rotation_g,
enum Rotation rotation_gH)
{ {
if (!dev_gyro || !dev_accel) { if (!dev_gyro || !dev_accel) {
return nullptr; return nullptr;
@ -406,7 +410,7 @@ AP_InertialSensor_Backend *AP_InertialSensor_LSM9DS0::probe(AP_InertialSensor &_
AP_InertialSensor_LSM9DS0 *sensor = AP_InertialSensor_LSM9DS0 *sensor =
new AP_InertialSensor_LSM9DS0(_imu, std::move(dev_gyro), std::move(dev_accel), new AP_InertialSensor_LSM9DS0(_imu, std::move(dev_gyro), std::move(dev_accel),
LSM9DS0_DRY_X_PIN, LSM9DS0_DRY_G_PIN, LSM9DS0_DRY_X_PIN, LSM9DS0_DRY_G_PIN,
rotation_a, rotation_g); rotation_a, rotation_g, rotation_gH);
if (!sensor || !sensor->_init_sensor()) { if (!sensor || !sensor->_init_sensor()) {
delete sensor; delete sensor;
return nullptr; return nullptr;
@ -456,16 +460,15 @@ bool AP_InertialSensor_LSM9DS0::_hardware_init()
return false; return false;
} }
uint8_t whoami; uint8_t tries, whoami;
uint8_t tries;
// set flag for reading registers // set flag for reading registers
_dev_gyro->set_read_flag(0x80); _dev_gyro->set_read_flag(0x80);
_dev_accel->set_read_flag(0x80); _dev_accel->set_read_flag(0x80);
whoami = _register_read_g(WHO_AM_I_G); whoami_g = _register_read_g(WHO_AM_I_G);
if (whoami != LSM9DS0_G_WHOAMI) { if (whoami_g != LSM9DS0_G_WHOAMI && whoami_g != LSM9DS0_G_WHOAMI_H) {
hal.console->printf("LSM9DS0: unexpected gyro WHOAMI 0x%x\n", (unsigned)whoami); hal.console->printf("LSM9DS0: unexpected gyro WHOAMI 0x%x\n", (unsigned)whoami_g);
goto fail_whoami; 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)); _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)); _accel_instance = _imu.register_accel(800, _dev_accel->get_bus_id_devtype(DEVTYPE_ACC_LSM303D));
set_gyro_orientation(_gyro_instance, _rotation_g); 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_orientation(_accel_instance, _rotation_a);
_set_accel_max_abs_offset(_accel_instance, 5.0f); _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_gyro,
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev_accel, AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev_accel,
enum Rotation rotation_a = ROTATION_NONE, 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: private:
AP_InertialSensor_LSM9DS0(AP_InertialSensor &imu, AP_InertialSensor_LSM9DS0(AP_InertialSensor &imu,
@ -27,7 +28,8 @@ private:
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev_accel, AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev_accel,
int drdy_pin_num_a, int drdy_pin_num_b, int drdy_pin_num_a, int drdy_pin_num_b,
enum Rotation rotation_a, enum Rotation rotation_a,
enum Rotation rotation_g); enum Rotation rotation_g,
enum Rotation rotation_gH);
struct PACKED sensor_raw_data { struct PACKED sensor_raw_data {
int16_t x; int16_t x;
@ -97,10 +99,14 @@ private:
uint8_t _gyro_instance; uint8_t _gyro_instance;
uint8_t _accel_instance; uint8_t _accel_instance;
// gyro whoami
uint8_t whoami_g;
/* /*
for boards that have a separate LSM303D and L3GD20 there can be for boards that have a separate LSM303D and L3GD20 there can be
different rotations for each different rotations for each
*/ */
enum Rotation _rotation_a; enum Rotation _rotation_a;
enum Rotation _rotation_g; enum Rotation _rotation_g; // for L3GD20
enum Rotation _rotation_gH; // for L3GD20H
}; };