From fb77d0739e3235d53c156e26930cbd122c3124eb Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Tue, 5 Sep 2017 18:58:35 -0700 Subject: [PATCH] AP_Compass: Remove unused time calls, stash the time in the read loop --- libraries/AP_Compass/AP_Compass.cpp | 3 ++- libraries/AP_Compass/AP_Compass_AK09916.cpp | 2 +- libraries/AP_Compass/AP_Compass_AK8963.cpp | 3 +-- libraries/AP_Compass/AP_Compass_BMM150.cpp | 4 +--- libraries/AP_Compass/AP_Compass_Backend.cpp | 2 +- libraries/AP_Compass/AP_Compass_Backend.h | 2 +- libraries/AP_Compass/AP_Compass_HIL.cpp | 2 +- libraries/AP_Compass/AP_Compass_HMC5843.cpp | 4 +--- libraries/AP_Compass/AP_Compass_IST8310.cpp | 4 +--- libraries/AP_Compass/AP_Compass_LIS3MDL.cpp | 2 +- libraries/AP_Compass/AP_Compass_LSM303D.cpp | 2 +- libraries/AP_Compass/AP_Compass_LSM9DS1.cpp | 3 +-- libraries/AP_Compass/AP_Compass_MMC3416.cpp | 2 +- libraries/AP_Compass/AP_Compass_QMC5883L.cpp | 4 +--- libraries/AP_Compass/AP_Compass_QURT.cpp | 4 +--- libraries/AP_Compass/AP_Compass_QURT.h | 1 - libraries/AP_Compass/AP_Compass_SITL.cpp | 2 +- libraries/AP_Compass/AP_Compass_UAVCAN.cpp | 3 +-- libraries/AP_Compass/AP_Compass_UAVCAN.h | 1 - libraries/AP_Compass/AP_Compass_qflight.cpp | 3 +-- 20 files changed, 19 insertions(+), 34 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index 9eb7864ee7..f1deacdef1 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -804,8 +804,9 @@ Compass::read(void) // call read on each of the backend. This call updates field[i] _backends[i]->read(); } + uint32_t time = AP_HAL::millis(); for (uint8_t i=0; i < COMPASS_MAX_INSTANCES; i++) { - _state[i].healthy = (AP_HAL::millis() - _state[i].last_update_ms < 500); + _state[i].healthy = (time - _state[i].last_update_ms < 500); } return healthy(); } diff --git a/libraries/AP_Compass/AP_Compass_AK09916.cpp b/libraries/AP_Compass/AP_Compass_AK09916.cpp index 095d8fa48f..d4954bc935 100644 --- a/libraries/AP_Compass/AP_Compass_AK09916.cpp +++ b/libraries/AP_Compass/AP_Compass_AK09916.cpp @@ -201,7 +201,7 @@ void AP_Compass_AK09916::timer() rotate_field(field, compass_instance); /* publish raw_field (uncorrected point sample) for calibration use */ - publish_raw_field(field, AP_HAL::micros(), compass_instance); + publish_raw_field(field, compass_instance); /* correct raw_field for known errors */ correct_field(field, compass_instance); diff --git a/libraries/AP_Compass/AP_Compass_AK8963.cpp b/libraries/AP_Compass/AP_Compass_AK8963.cpp index 9c61933dcc..6922da9d14 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.cpp +++ b/libraries/AP_Compass/AP_Compass_AK8963.cpp @@ -219,7 +219,6 @@ void AP_Compass_AK8963::_update() { struct sample_regs regs; Vector3f raw_field; - uint32_t time_us = AP_HAL::micros(); if (!_bus->block_read(AK8963_HXL, (uint8_t *) ®s, sizeof(regs))) { return; @@ -245,7 +244,7 @@ void AP_Compass_AK8963::_update() rotate_field(raw_field, _compass_instance); // publish raw_field (uncorrected point sample) for calibration use - publish_raw_field(raw_field, time_us, _compass_instance); + publish_raw_field(raw_field, _compass_instance); // correct raw_field for known errors correct_field(raw_field, _compass_instance); diff --git a/libraries/AP_Compass/AP_Compass_BMM150.cpp b/libraries/AP_Compass/AP_Compass_BMM150.cpp index 59e9190100..e31b5426ff 100644 --- a/libraries/AP_Compass/AP_Compass_BMM150.cpp +++ b/libraries/AP_Compass/AP_Compass_BMM150.cpp @@ -286,8 +286,6 @@ int16_t AP_Compass_BMM150::_compensate_z(int16_t z, uint32_t rhall) void AP_Compass_BMM150::_update() { - const uint32_t time_usec = AP_HAL::micros(); - le16_t data[4]; bool ret = _dev->read_registers(DATA_X_LSB_REG, (uint8_t *) &data, sizeof(data)); @@ -323,7 +321,7 @@ void AP_Compass_BMM150::_update() rotate_field(raw_field, _compass_instance); /* publish raw_field (uncorrected point sample) for calibration use */ - publish_raw_field(raw_field, time_usec, _compass_instance); + publish_raw_field(raw_field, _compass_instance); /* correct raw_field for known errors */ correct_field(raw_field, _compass_instance); diff --git a/libraries/AP_Compass/AP_Compass_Backend.cpp b/libraries/AP_Compass/AP_Compass_Backend.cpp index 5873317d99..8d52916109 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.cpp +++ b/libraries/AP_Compass/AP_Compass_Backend.cpp @@ -26,7 +26,7 @@ void AP_Compass_Backend::rotate_field(Vector3f &mag, uint8_t instance) } } -void AP_Compass_Backend::publish_raw_field(const Vector3f &mag, uint32_t time_us, uint8_t instance) +void AP_Compass_Backend::publish_raw_field(const Vector3f &mag, uint8_t instance) { Compass::mag_state &state = _compass._state[instance]; diff --git a/libraries/AP_Compass/AP_Compass_Backend.h b/libraries/AP_Compass/AP_Compass_Backend.h index f5da1cb6f6..a7c4b9733a 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.h +++ b/libraries/AP_Compass/AP_Compass_Backend.h @@ -81,7 +81,7 @@ protected: */ void rotate_field(Vector3f &mag, uint8_t instance); - void publish_raw_field(const Vector3f &mag, uint32_t time_us, uint8_t instance); + void publish_raw_field(const Vector3f &mag, uint8_t instance); void correct_field(Vector3f &mag, uint8_t i); void publish_filtered_field(const Vector3f &mag, uint8_t instance); void set_last_update_usec(uint32_t last_update, uint8_t instance); diff --git a/libraries/AP_Compass/AP_Compass_HIL.cpp b/libraries/AP_Compass/AP_Compass_HIL.cpp index 2c5c2757e2..188ed82ad7 100644 --- a/libraries/AP_Compass/AP_Compass_HIL.cpp +++ b/libraries/AP_Compass/AP_Compass_HIL.cpp @@ -63,7 +63,7 @@ void AP_Compass_HIL::read() uint8_t compass_instance = _compass_instance[i]; Vector3f field = _compass._hil.field[compass_instance]; rotate_field(field, compass_instance); - publish_raw_field(field, AP_HAL::micros(), compass_instance); + publish_raw_field(field, compass_instance); correct_field(field, compass_instance); uint32_t saved_last_update = _compass.last_update_usec(compass_instance); publish_filtered_field(field, compass_instance); diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.cpp b/libraries/AP_Compass/AP_Compass_HMC5843.cpp index ac0d3caf9f..ddcce28727 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.cpp +++ b/libraries/AP_Compass/AP_Compass_HMC5843.cpp @@ -236,8 +236,6 @@ void AP_Compass_HMC5843::_timer() return; } - uint32_t tnow = AP_HAL::micros(); - // the _mag_N values are in the range -2048 to 2047, so we can // accumulate up to 15 of them in an int16_t. Let's make it 14 // for ease of calculation. We expect to do reads at 10Hz, and @@ -256,7 +254,7 @@ void AP_Compass_HMC5843::_timer() rotate_field(raw_field, _compass_instance); // publish raw_field (uncorrected point sample) for calibration use - publish_raw_field(raw_field, tnow, _compass_instance); + publish_raw_field(raw_field, _compass_instance); // correct raw_field for known errors correct_field(raw_field, _compass_instance); diff --git a/libraries/AP_Compass/AP_Compass_IST8310.cpp b/libraries/AP_Compass/AP_Compass_IST8310.cpp index 1ece7cfc1c..de9001ea53 100644 --- a/libraries/AP_Compass/AP_Compass_IST8310.cpp +++ b/libraries/AP_Compass/AP_Compass_IST8310.cpp @@ -204,8 +204,6 @@ void AP_Compass_IST8310::timer() return; } - uint32_t now = AP_HAL::micros(); - start_conversion(); /* same period, but start counting from now */ @@ -236,7 +234,7 @@ void AP_Compass_IST8310::timer() rotate_field(field, _instance); /* publish raw_field (uncorrected point sample) for calibration use */ - publish_raw_field(field, now, _instance); + publish_raw_field(field, _instance); /* correct raw_field for known errors */ correct_field(field, _instance); diff --git a/libraries/AP_Compass/AP_Compass_LIS3MDL.cpp b/libraries/AP_Compass/AP_Compass_LIS3MDL.cpp index 1ba0df1072..c3442047d9 100644 --- a/libraries/AP_Compass/AP_Compass_LIS3MDL.cpp +++ b/libraries/AP_Compass/AP_Compass_LIS3MDL.cpp @@ -164,7 +164,7 @@ void AP_Compass_LIS3MDL::timer() rotate_field(field, compass_instance); /* publish raw_field (uncorrected point sample) for calibration use */ - publish_raw_field(field, AP_HAL::micros(), compass_instance); + publish_raw_field(field, compass_instance); /* correct raw_field for known errors */ correct_field(field, compass_instance); diff --git a/libraries/AP_Compass/AP_Compass_LSM303D.cpp b/libraries/AP_Compass/AP_Compass_LSM303D.cpp index 47e0e088c7..c2ab484dd1 100644 --- a/libraries/AP_Compass/AP_Compass_LSM303D.cpp +++ b/libraries/AP_Compass/AP_Compass_LSM303D.cpp @@ -355,7 +355,7 @@ void AP_Compass_LSM303D::_update() rotate_field(raw_field, _compass_instance); // publish raw_field (uncorrected point sample) for calibration use - publish_raw_field(raw_field, AP_HAL::micros(), _compass_instance); + publish_raw_field(raw_field, _compass_instance); // correct raw_field for known errors correct_field(raw_field, _compass_instance); diff --git a/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp b/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp index 3c076c003d..fbabb7bdba 100644 --- a/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp +++ b/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp @@ -141,7 +141,6 @@ void AP_Compass_LSM9DS1::_update(void) { struct sample_regs regs; Vector3f raw_field; - uint32_t time_us = AP_HAL::micros(); if (!_block_read(LSM9DS1M_STATUS_REG_M, (uint8_t *) ®s, sizeof(regs))) { return; @@ -163,7 +162,7 @@ void AP_Compass_LSM9DS1::_update(void) rotate_field(raw_field, _compass_instance); // publish raw_field (uncorrected point sample) for calibration use - publish_raw_field(raw_field, time_us, _compass_instance); + publish_raw_field(raw_field, _compass_instance); // correct raw_field for known errors correct_field(raw_field, _compass_instance); diff --git a/libraries/AP_Compass/AP_Compass_MMC3416.cpp b/libraries/AP_Compass/AP_Compass_MMC3416.cpp index f95a6c33c0..e140977c3c 100644 --- a/libraries/AP_Compass/AP_Compass_MMC3416.cpp +++ b/libraries/AP_Compass/AP_Compass_MMC3416.cpp @@ -298,7 +298,7 @@ void AP_Compass_MMC3416::accumulate_field(Vector3f &field) rotate_field(field, compass_instance); /* publish raw_field (uncorrected point sample) for calibration use */ - publish_raw_field(field, AP_HAL::micros(), compass_instance); + publish_raw_field(field, compass_instance); /* correct raw_field for known errors */ correct_field(field, compass_instance); diff --git a/libraries/AP_Compass/AP_Compass_QMC5883L.cpp b/libraries/AP_Compass/AP_Compass_QMC5883L.cpp index ba265b8ee1..cfaf0811a4 100644 --- a/libraries/AP_Compass/AP_Compass_QMC5883L.cpp +++ b/libraries/AP_Compass/AP_Compass_QMC5883L.cpp @@ -181,8 +181,6 @@ void AP_Compass_QMC5883L::timer() return ; } - uint32_t now = AP_HAL::micros(); - auto x = -static_cast(le16toh(buffer.rx)); auto y = static_cast(le16toh(buffer.ry)); auto z = -static_cast(le16toh(buffer.rz)); @@ -204,7 +202,7 @@ void AP_Compass_QMC5883L::timer() rotate_field(field, _instance); /* publish raw_field (uncorrected point sample) for calibration use */ - publish_raw_field(field, now, _instance); + publish_raw_field(field, _instance); /* correct raw_field for known errors */ correct_field(field, _instance); diff --git a/libraries/AP_Compass/AP_Compass_QURT.cpp b/libraries/AP_Compass/AP_Compass_QURT.cpp index e819fe3f5c..d16cdc8c2a 100644 --- a/libraries/AP_Compass/AP_Compass_QURT.cpp +++ b/libraries/AP_Compass/AP_Compass_QURT.cpp @@ -72,8 +72,6 @@ void AP_Compass_QURT::timer_update(void) return; } - last_timestamp = data.timestamp; - Vector3f raw_field(data.mag_raw[0], data.mag_raw[1], -data.mag_raw[2]); @@ -82,7 +80,7 @@ void AP_Compass_QURT::timer_update(void) rotate_field(raw_field, instance); // publish raw_field (uncorrected point sample) for calibration use - publish_raw_field(raw_field, data.timestamp, instance); + publish_raw_field(raw_field, instance); // correct raw_field for known errors correct_field(raw_field, instance); diff --git a/libraries/AP_Compass/AP_Compass_QURT.h b/libraries/AP_Compass/AP_Compass_QURT.h index c0dc1af723..c32b6ed43f 100644 --- a/libraries/AP_Compass/AP_Compass_QURT.h +++ b/libraries/AP_Compass/AP_Compass_QURT.h @@ -20,7 +20,6 @@ private: uint8_t instance; Vector3f sum; uint32_t count; - uint64_t last_timestamp; }; diff --git a/libraries/AP_Compass/AP_Compass_SITL.cpp b/libraries/AP_Compass/AP_Compass_SITL.cpp index 6dd2f7cca2..76e140c9ea 100644 --- a/libraries/AP_Compass/AP_Compass_SITL.cpp +++ b/libraries/AP_Compass/AP_Compass_SITL.cpp @@ -70,7 +70,7 @@ void AP_Compass_SITL::_timer() for (uint8_t i=0; inum_samples; i++) { DSPBuffer::MAG::BUF &b = magbuf->buf[i]; @@ -91,7 +90,7 @@ void AP_Compass_QFLIGHT::timer_update(void) rotate_field(raw_field, instance); // publish raw_field (uncorrected point sample) for calibration use - publish_raw_field(raw_field, time_us, instance); + publish_raw_field(raw_field, instance); // correct raw_field for known errors correct_field(raw_field, instance);