aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_InertialSensor: enable temperature sensor on LSM9DS0

Browse Source 
this allows for temperature calibration of the LSM303D in CubeBlack
and Pixhawk1
This commit is contained in:
Andrew Tridgell 2021-01-09 09:24:59 +11:00 committed by Peter Barker
parent 0c156e324b
commit 6f6f89e5aa
2 changed files with 18 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp
View File

@ -387,6 +387,7 @@ AP_InertialSensor_LSM9DS0::AP_InertialSensor_LSM9DS0(AP_InertialSensor &imu,
, _rotation_a(rotation_a) , _rotation_a(rotation_a)
, _rotation_g(rotation_g) , _rotation_g(rotation_g)
, _rotation_gH(rotation_gH) , _rotation_gH(rotation_gH)
, _temp_filter(80, 1)
{ {
} }
@ -639,6 +640,9 @@ void AP_InertialSensor_LSM9DS0::_accel_init()
/* Accel data ready on INT1 */ /* Accel data ready on INT1 */
_register_write_xm(CTRL_REG3_XM, CTRL_REG3_XM_P1_DRDYA, true); _register_write_xm(CTRL_REG3_XM, CTRL_REG3_XM_P1_DRDYA, true);
hal.scheduler->delay(1); hal.scheduler->delay(1);
// enable temperature sensor
_register_write_xm(CTRL_REG5_XM, _register_read_xm(CTRL_REG5_XM) | 0x80, false);
} }
void AP_InertialSensor_LSM9DS0::_set_gyro_scale(gyro_scale scale) void AP_InertialSensor_LSM9DS0::_set_gyro_scale(gyro_scale scale)
@ -725,7 +729,6 @@ void AP_InertialSensor_LSM9DS0::_read_data_transaction_a()
const uint8_t reg = OUT_X_L_A | 0xC0; const uint8_t reg = OUT_X_L_A | 0xC0;
if (!_dev_accel->transfer(&reg, 1, (uint8_t *) &raw_data, sizeof(raw_data))) { if (!_dev_accel->transfer(&reg, 1, (uint8_t *) &raw_data, sizeof(raw_data))) {
hal.console->printf("LSM9DS0: error reading accelerometer\n");
return; return;
} }
@ -734,6 +737,15 @@ void AP_InertialSensor_LSM9DS0::_read_data_transaction_a()
_rotate_and_correct_accel(_accel_instance, accel_data); _rotate_and_correct_accel(_accel_instance, accel_data);
_notify_new_accel_raw_sample(_accel_instance, accel_data, AP_HAL::micros64()); _notify_new_accel_raw_sample(_accel_instance, accel_data, AP_HAL::micros64());
// read temperature every 10th sample
if (_temp_counter++ % 10 == 0) {
int16_t traw;
const uint8_t regtemp = OUT_TEMP_L_XM | 0xC0;
if (_dev_accel->transfer(&regtemp, 1, (uint8_t *)&traw, sizeof(traw))) {
_temperature = _temp_filter.apply(traw * 0.125 + 25);
}
}
} }
/* /*
@ -745,7 +757,6 @@ void AP_InertialSensor_LSM9DS0::_read_data_transaction_g()
const uint8_t reg = OUT_X_L_G | 0xC0; const uint8_t reg = OUT_X_L_G | 0xC0;
if (!_dev_gyro->transfer(&reg, 1, (uint8_t *) &raw_data, sizeof(raw_data))) { if (!_dev_gyro->transfer(&reg, 1, (uint8_t *) &raw_data, sizeof(raw_data))) {
hal.console->printf("LSM9DS0: error reading gyroscope\n");
return; return;
} }
@ -761,6 +772,7 @@ bool AP_InertialSensor_LSM9DS0::update()
{ {
update_gyro(_gyro_instance); update_gyro(_gyro_instance);
update_accel(_accel_instance); update_accel(_accel_instance);
_publish_temperature(_accel_instance, _temperature);
return true; return true;
} }

4
libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h
View File

@ -7,6 +7,7 @@
#include "AP_InertialSensor.h" #include "AP_InertialSensor.h"
#include "AP_InertialSensor_Backend.h" #include "AP_InertialSensor_Backend.h"
#include <Filter/LowPassFilter2p.h>
class AP_InertialSensor_LSM9DS0 : public AP_InertialSensor_Backend class AP_InertialSensor_LSM9DS0 : public AP_InertialSensor_Backend
{ {
@ -98,6 +99,9 @@ private:
int _drdy_pin_num_g; int _drdy_pin_num_g;
uint8_t _gyro_instance; uint8_t _gyro_instance;
uint8_t _accel_instance; uint8_t _accel_instance;
float _temperature;
uint8_t _temp_counter;
LowPassFilter2pFloat _temp_filter;
// gyro whoami // gyro whoami
uint8_t whoami_g; uint8_t whoami_g;