fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 12:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

PX4Accelerometer/PX4Gyroscope update FIFO case to trapezoidal integration

Browse Source
This commit is contained in:
Daniel Agar
2020-01-22 18:35:41 -05:00
parent 697dbfb9f8
commit 009ba638f5
9 changed files with 215 additions and 214 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/imu/invensense/icm20602/ICM20602.cpp
+7 -6
View File
@@ -300,6 +300,11 @@ void ICM20602::Run()
{
perf_count(_interval_perf);
// use timestamp from the data ready interrupt if available,
// otherwise use the time now roughly corresponding with the last sample we'll pull from the FIFO
const hrt_abstime timestamp_sample = (hrt_elapsed_time(&_time_data_ready) < FIFO_INTERVAL) ? _time_data_ready :
hrt_absolute_time();
// read FIFO count
uint8_t fifo_count_buf[3] {};
fifo_count_buf[0] = static_cast<uint8_t>(Register::FIFO_COUNTH) | DIR_READ;
@@ -351,15 +356,11 @@ void ICM20602::Run()
perf_end(_transfer_perf);
static constexpr uint32_t gyro_dt = FIFO_INTERVAL / FIFO_GYRO_SAMPLES;
// estimate timestamp of first sample in the FIFO from number of samples and fill rate
const hrt_abstime timestamp_sample = _time_data_ready - ((samples - 1) * gyro_dt);
PX4Accelerometer::FIFOSample accel{};
PX4Accelerometer::FIFOSample accel;
accel.timestamp_sample = timestamp_sample;
accel.dt = FIFO_INTERVAL / FIFO_ACCEL_SAMPLES;
PX4Gyroscope::FIFOSample gyro{};
PX4Gyroscope::FIFOSample gyro;
gyro.timestamp_sample = timestamp_sample;
gyro.samples = samples;
gyro.dt = FIFO_INTERVAL / FIFO_GYRO_SAMPLES;
src/drivers/imu/invensense/icm20608-g/ICM20608G.cpp
+7 -6
View File
@@ -276,6 +276,11 @@ void ICM20608G::Run()
{
perf_count(_interval_perf);
// use timestamp from the data ready interrupt if available,
// otherwise use the time now roughly corresponding with the last sample we'll pull from the FIFO
const hrt_abstime timestamp_sample = (hrt_elapsed_time(&_time_data_ready) < FIFO_INTERVAL) ? _time_data_ready :
hrt_absolute_time();
// read FIFO count
uint8_t fifo_count_buf[3] {};
fifo_count_buf[0] = static_cast<uint8_t>(Register::FIFO_COUNTH) | DIR_READ;
@@ -327,15 +332,11 @@ void ICM20608G::Run()
perf_end(_transfer_perf);
static constexpr uint32_t gyro_dt = FIFO_INTERVAL / FIFO_GYRO_SAMPLES;
// estimate timestamp of first sample in the FIFO from number of samples and fill rate
const hrt_abstime timestamp_sample = _time_data_ready - ((samples - 1) * gyro_dt);
PX4Accelerometer::FIFOSample accel{};
PX4Accelerometer::FIFOSample accel;
accel.timestamp_sample = timestamp_sample;
accel.dt = FIFO_INTERVAL / FIFO_ACCEL_SAMPLES;
PX4Gyroscope::FIFOSample gyro{};
PX4Gyroscope::FIFOSample gyro;
gyro.timestamp_sample = timestamp_sample;
gyro.samples = samples;
gyro.dt = FIFO_INTERVAL / FIFO_GYRO_SAMPLES;
src/drivers/imu/st/ism330dlc/ISM330DLC.cpp
+16 -13
View File
@@ -40,6 +40,8 @@ using namespace ST_ISM330DLC;
static constexpr int16_t combine(uint8_t lsb, uint8_t msb) { return (msb << 8u) | lsb; }
static constexpr uint32_t FIFO_INTERVAL{1000}; // 1000 us / 1000 Hz interval
ISM330DLC::ISM330DLC(int bus, uint32_t device, enum Rotation rotation) :
SPI(MODULE_NAME, nullptr, bus, device, SPIDEV_MODE3, SPI_SPEED),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
@@ -53,8 +55,8 @@ ISM330DLC::ISM330DLC(int bus, uint32_t device, enum Rotation rotation) :
_px4_accel.set_sample_rate(ST_ISM330DLC::LA_ODR);
_px4_gyro.set_sample_rate(ST_ISM330DLC::G_ODR);
_px4_accel.set_update_rate(1000000 / _fifo_interval);
_px4_gyro.set_update_rate(1000000 / _fifo_interval);
_px4_accel.set_update_rate(1000000 / FIFO_INTERVAL);
_px4_gyro.set_update_rate(1000000 / FIFO_INTERVAL);
}
ISM330DLC::~ISM330DLC()
@@ -227,8 +229,7 @@ void ISM330DLC::Start()
RegisterWrite(Register::INT1_CTRL, INT1_CTRL_BIT::INT1_FULL_FLAG | INT1_CTRL_BIT::INT1_FIFO_OVR |
INT1_CTRL_BIT::INT1_FTH);
#else
ScheduleOnInterval(_fifo_interval, _fifo_interval);
ScheduleOnInterval(FIFO_INTERVAL, FIFO_INTERVAL);
#endif
}
@@ -248,8 +249,12 @@ void ISM330DLC::Run()
{
perf_count(_interval_perf);
// use timestamp from the data ready interrupt if available,
// otherwise use the time now roughly corresponding with the last sample we'll pull from the FIFO
const hrt_abstime timestamp_sample = (hrt_elapsed_time(&_time_data_ready) < FIFO_INTERVAL) ? _time_data_ready :
hrt_absolute_time();
// Number of unread words (16-bit axes) stored in FIFO.
const hrt_abstime timestamp_fifo_level = hrt_absolute_time();
const uint8_t fifo_words = RegisterRead(Register::FIFO_STATUS1);
// check for FIFO status
@@ -311,19 +316,15 @@ void ISM330DLC::Run()
perf_end(_transfer_perf);
static constexpr uint32_t gyro_dt = 1000000 / ST_ISM330DLC::G_ODR;
// estimate timestamp of first sample in the FIFO from number of samples and fill rate
const hrt_abstime timestamp_sample = timestamp_fifo_level - ((samples - 1) * gyro_dt);
PX4Accelerometer::FIFOSample accel{};
PX4Accelerometer::FIFOSample accel;
accel.timestamp_sample = timestamp_sample;
accel.samples = samples;
accel.dt = gyro_dt;
accel.dt = 1000000 / ST_ISM330DLC::LA_ODR;
PX4Gyroscope::FIFOSample gyro{};
PX4Gyroscope::FIFOSample gyro;
gyro.timestamp_sample = timestamp_sample;
gyro.samples = samples;
gyro.dt = gyro_dt;
gyro.dt = 1000000 / ST_ISM330DLC::G_ODR;
for (int i = 0; i < samples; i++) {
const FIFO::DATA &fifo_sample = report->f[i];
@@ -366,6 +367,8 @@ void ISM330DLC::PrintInfo()
perf_print_counter(_fifo_empty_perf);
perf_print_counter(_fifo_overflow_perf);
perf_print_counter(_fifo_reset_perf);
perf_print_counter(_drdy_count_perf);
perf_print_counter(_drdy_interval_perf);
_px4_accel.print_status();
_px4_gyro.print_status();
src/drivers/imu/st/ism330dlc/ISM330DLC.hpp
-2
View File
@@ -84,8 +84,6 @@ private:
PX4Accelerometer _px4_accel;
PX4Gyroscope _px4_gyro;
static constexpr uint32_t _fifo_interval{1000}; // 1000 us sample interval
perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": run interval")};
perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": transfer")};
perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": fifo empty")};
src/drivers/imu/st/ism330dlc/ST_ISM330DLC_Registers.hpp
+1 -1
View File
@@ -53,7 +53,7 @@ static constexpr uint8_t Bit7 = (1 << 7);
namespace ST_ISM330DLC
{
static constexpr uint8_t DIR_READ = 0x80;
static constexpr uint8_t DIR_READ = 0x80;
static constexpr uint8_t ISM330DLC_WHO_AM_I = 0b01101010; // Who I am ID