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

AP_Compass: IST8310: account for errors in measurment requests 

If we don't recover for errors in the request for new sample, we may get
stuck with no sample anymore. Recover from bad transfers.
This commit is contained in:
Lucas De Marchi 2017-02-24 18:16:45 -08:00 committed by Andrew Tridgell
parent 18ba1aa166
commit b339050e45
2 changed files with 44 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
libraries/AP_Compass/AP_Compass_IST8310.cpp
View File

@ -42,6 +42,8 @@
#define PDCNTL_REG 0x42
#define NORMAL_PULSE_DURATION 0xC0
#define SAMPLING_PERIOD_USEC (10 * USEC_PER_MSEC)
extern const AP_HAL::HAL &hal;
AP_Compass_Backend *AP_Compass_IST8310::probe(Compass &compass,
@ -110,9 +112,13 @@ bool AP_Compass_IST8310::init()
_dev->set_device_type(DEVTYPE_IST8310);
set_dev_id(_instance, _dev->get_bus_id());
_dev->register_periodic_callback(10 * USEC_PER_MSEC,
_dev->register_periodic_callback(SAMPLING_PERIOD_USEC,
FUNCTOR_BIND_MEMBER(&AP_Compass_IST8310::timer, void));
_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");
return true;
fail:
@ -122,13 +128,21 @@ fail:
void AP_Compass_IST8310::start_conversion()
{
_dev->write_register(CNTL1_REG,
SINGLE_MEASUREMENT_MODE);
if (!_dev->write_register(CNTL1_REG, SINGLE_MEASUREMENT_MODE)) {
hal.util->perf_count(_perf_xfer_err);
_need_start = true;
return;
}
_need_start = false;
}
void AP_Compass_IST8310::timer()
{
bool ret = false;
if (_need_start) {
start_conversion();
return;
}
struct PACKED {
uint8_t status;
@ -137,20 +151,34 @@ void AP_Compass_IST8310::timer()
le16_t rz;
} buffer;
ret = _dev->read_registers(STAT1_REG, (uint8_t *) &buffer, sizeof(buffer));
bool ret = _dev->read_registers(STAT1_REG, (uint8_t *) &buffer, sizeof(buffer));
if (!ret) {
/* We're going to be back on the next iteration either way */
hal.util->perf_count(_perf_xfer_err);
return;
}
auto status = buffer.status;
uint32_t now = AP_HAL::micros();
if (!(status & 0x01)) {
/* We're not ready yet */
return;
hal.util->perf_count(_perf_not_ready);
/*
* Sensor is in a wrong state or something went really wrong: try to
* request a sample again
*/
if (now > _last_measurement_usec + 2 * SAMPLING_PERIOD_USEC) {
hal.util->perf_count(_perf_restart);
start_conversion();
}
return;
}
_last_measurement_usec = now;
start_conversion();
auto x = static_cast<int16_t>(le16toh(buffer.rx));
auto y = static_cast<int16_t>(le16toh(buffer.ry));
auto z = static_cast<int16_t>(le16toh(buffer.rz));
@ -162,7 +190,7 @@ void AP_Compass_IST8310::timer()
rotate_field(field, _instance);
/* publish raw_field (uncorrected point sample) for calibration use */
publish_raw_field(field, AP_HAL::micros(), _instance);
publish_raw_field(field, now, _instance);
/* correct raw_field for known errors */
correct_field(field, _instance);
@ -172,8 +200,6 @@ void AP_Compass_IST8310::timer()
_accum_count++;
_sem->give();
}
start_conversion();
}
void AP_Compass_IST8310::read()

6
libraries/AP_Compass/AP_Compass_IST8310.h
View File

@ -47,8 +47,14 @@ private:
AP_HAL::OwnPtr<AP_HAL::Device> _dev;
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;
Vector3f _accum = Vector3f();
uint32_t _accum_count = 0;
uint32_t _last_measurement_usec;
enum Rotation _rotation;
uint8_t _instance;
bool _need_start;
};