From f0ebe8f7807f3e185215903664552c8d0400fcbd Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Fri, 23 Feb 2018 11:25:22 +0100 Subject: [PATCH] LP-580 QMC5883L: added DRDY timeout, so that driver can reinit chip --- flight/pios/common/pios_qmc5883l.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/flight/pios/common/pios_qmc5883l.c b/flight/pios/common/pios_qmc5883l.c index 66718b37f..5f871d58b 100644 --- a/flight/pios/common/pios_qmc5883l.c +++ b/flight/pios/common/pios_qmc5883l.c @@ -75,6 +75,7 @@ #define PIOS_QMC5883L_I2C_CONFIG_RETRY_DELAY 1000000 #define PIOS_QMC5883L_I2C_CONFIG_DATA_DELAY 10000 +#define PIOS_QMC5883L_I2C_CONFIG_DATA_TIMEOUT (PIOS_QMC5883L_I2C_CONFIG_DATA_DELAY * 10) #define PIOS_QMC5883L_I2C_RETRIES 2 enum pios_qmc5883l_dev_magic { @@ -249,7 +250,7 @@ static void PIOS_QMC5883L_driver_Reset(__attribute__((unused)) uintptr_t context static void PIOS_QMC5883L_driver_get_scale(float *scales, uint8_t size, __attribute__((unused)) uintptr_t context) { PIOS_Assert(size > 0); - scales[0] = 0.5; + scales[0] = 0.35f; } static void PIOS_QMC5883L_driver_fetch(void *data, __attribute__((unused)) uint8_t size, uintptr_t context) @@ -293,6 +294,9 @@ static bool PIOS_QMC5883L_driver_poll(uintptr_t context) // Is it ready? if (!(status & PIOS_QMC5883L_STATUS_DRDY)) { + if (PIOS_DELAY_DiffuS(dev->readTime) > PIOS_QMC5883L_I2C_CONFIG_DATA_TIMEOUT) { // the sensor has reset and it is not in continuous mode anymore + dev->sensorIsAlive = false; + } return false; }