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

bmi055: gyro don't publish duplicates

Browse Source
This commit is contained in:
Daniel Agar
2020-05-08 10:41:12 -04:00
parent c5cbc7725d
commit e0fc404f91
2 changed files with 19 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/imu/bmi055/BMI055_gyro.cpp
+16 -1
View File
@@ -55,7 +55,8 @@ BMI055_gyro::BMI055_gyro(I2CSPIBusOption bus_option, int bus, const char *path_g
_px4_gyro(get_device_id(), (external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT), rotation),
_sample_perf(perf_alloc(PC_ELAPSED, "bmi055_gyro_read")),
_bad_transfers(perf_alloc(PC_COUNT, "bmi055_gyro_bad_transfers")),
_bad_registers(perf_alloc(PC_COUNT, "bmi055_gyro_bad_registers"))
_bad_registers(perf_alloc(PC_COUNT, "bmi055_gyro_bad_registers")),
_duplicates(perf_alloc(PC_COUNT, "bmi055_gyro_duplicates"))
{
_px4_gyro.set_update_rate(BMI055_GYRO_DEFAULT_RATE);
}
@@ -65,6 +66,7 @@ BMI055_gyro::~BMI055_gyro()
perf_free(_sample_perf);
perf_free(_bad_transfers);
perf_free(_bad_registers);
perf_free(_duplicates);
}
int
@@ -360,6 +362,18 @@ BMI055_gyro::RunImpl()
return;
}
// don't publish duplicated reads
if ((report.gyro_x == _gyro_prev[0]) && (report.gyro_y == _gyro_prev[1]) && (report.gyro_z == _gyro_prev[2])) {
perf_end(_sample_perf);
perf_count(_duplicates);
return;
} else {
_gyro_prev[0] = report.gyro_x;
_gyro_prev[1] = report.gyro_y;
_gyro_prev[2] = report.gyro_z;
}
// report the error count as the sum of the number of bad
// transfers and bad register reads. This allows the higher
// level code to decide if it should use this sensor based on
@@ -402,6 +416,7 @@ BMI055_gyro::print_status()
perf_print_counter(_sample_perf);
perf_print_counter(_bad_transfers);
perf_print_counter(_bad_registers);
perf_print_counter(_duplicates);
::printf("checked_next: %u\n", _checked_next);
src/drivers/imu/bmi055/BMI055_gyro.hpp
+3
View File
@@ -162,6 +162,7 @@ private:
perf_counter_t _sample_perf;
perf_counter_t _bad_transfers;
perf_counter_t _bad_registers;
perf_counter_t _duplicates;
// this is used to support runtime checking of key
// configuration registers to detect SPI bus errors and sensor
@@ -171,6 +172,8 @@ private:
uint8_t _checked_values[BMI055_GYRO_NUM_CHECKED_REGISTERS];
uint8_t _checked_bad[BMI055_GYRO_NUM_CHECKED_REGISTERS];
int16_t _gyro_prev[3] {};
/**
* Reset chip.
*