Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: IST8310: use FSR to reject samples

This commit is contained in:
Lucas De Marchi 2017-03-01 01:32:59 -08:00 committed by Andrew Tridgell
parent d84851d8fa
commit ce1a13aa8f
2 changed files with 25 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_Compass/AP_Compass_IST8310.cpp
View File

@ -53,6 +53,21 @@
#define SAMPLING_PERIOD_USEC (10 * USEC_PER_MSEC)
/*
* FSR:
* x, y: +- 1600 µT
* z: +- 2500 µT
*
* Resolution according to datasheet is 0.3µT/LSB
*/
#define IST8310_RESOLUTION 0.3
static const int16_t IST8310_MAX_VAL_XY = (1600 / IST8310_RESOLUTION) + 1;
static const int16_t IST8310_MIN_VAL_XY = -IST8310_MAX_VAL_XY;
static const int16_t IST8310_MAX_VAL_Z = (2500 / IST8310_RESOLUTION) + 1;
static const int16_t IST8310_MIN_VAL_Z = -IST8310_MAX_VAL_Z;
extern const AP_HAL::HAL &hal;
AP_Compass_Backend *AP_Compass_IST8310::probe(Compass &compass,
@ -149,6 +164,7 @@ bool AP_Compass_IST8310::init()
_perf_xfer_err = hal.util->perf_alloc(AP_HAL::Util::PC_COUNT, "IST8310_xfer_err");
_perf_not_ready = hal.util->perf_alloc(AP_HAL::Util::PC_COUNT, "IST8310_not_ready");
_perf_restart = hal.util->perf_alloc(AP_HAL::Util::PC_COUNT, "IST8310_restart");
_perf_bad_data = hal.util->perf_alloc(AP_HAL::Util::PC_COUNT, "IST8310_bad_data");
return true;
@ -213,10 +229,14 @@ void AP_Compass_IST8310::timer()
start_conversion();
// buffer data is 14bits. Check for invalid data and drop packet
if (x > 8191 || x < -8192 ||
y > 8191 || y < -8192 ||
z > 8191 || z < -8192) {
/*
* Check if value makes sense according to the FSR and Resolution of
* this sensor, discarding outliers
*/
if (x > IST8310_MAX_VAL_XY || x < IST8310_MIN_VAL_XY ||
y > IST8310_MAX_VAL_XY || y < IST8310_MIN_VAL_XY ||
z > IST8310_MAX_VAL_Z || z < IST8310_MIN_VAL_Z) {
hal.util->perf_count(_perf_bad_data);
return;
}

1
libraries/AP_Compass/AP_Compass_IST8310.h
View File

@ -50,6 +50,7 @@ private:
AP_HAL::Util::perf_counter_t _perf_xfer_err;
AP_HAL::Util::perf_counter_t _perf_not_ready;
AP_HAL::Util::perf_counter_t _perf_restart;
AP_HAL::Util::perf_counter_t _perf_bad_data;
Vector3f _accum = Vector3f();
uint32_t _accum_count = 0;