AP_Compass: Remove unused time calls, stash the time in the read loop

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();
 }

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);

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 *) &regs, 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);

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);

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];
 

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);

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);

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);

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);

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);

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);

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 *) &regs, 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);

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);

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<int16_t>(le16toh(buffer.rx));
     auto y = static_cast<int16_t>(le16toh(buffer.ry));
     auto z = -static_cast<int16_t>(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);

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);

libraries/AP_Compass/AP_Compass_QURT.h

@@ -20,7 +20,6 @@ private:
     uint8_t  instance;
     Vector3f sum;
     uint32_t count;
-    uint64_t last_timestamp;
 };
 
 

libraries/AP_Compass/AP_Compass_SITL.cpp

@@ -70,7 +70,7 @@ void AP_Compass_SITL::_timer()
 
     for (uint8_t i=0; i<SITL_NUM_COMPASSES; i++) {
         rotate_field(new_mag_data, _compass_instance[i]);
-        publish_raw_field(new_mag_data, AP_HAL::micros(), _compass_instance[i]);
+        publish_raw_field(new_mag_data, _compass_instance[i]);
         correct_field(new_mag_data, _compass_instance[i]);
     }
 

libraries/AP_Compass/AP_Compass_UAVCAN.cpp

@@ -153,9 +153,8 @@ void AP_Compass_UAVCAN::handle_mag_msg(Vector3f &mag)
     // rotate raw_field from sensor frame to body frame
     rotate_field(raw_field, _instance);
 
-    _last_timestamp = AP_HAL::micros64();
     // publish raw_field (uncorrected point sample) for calibration use
-    publish_raw_field(raw_field, (uint32_t) _last_timestamp, _instance);
+    publish_raw_field(raw_field, _instance);
 
     // correct raw_field for known errors
     correct_field(raw_field, _instance);

libraries/AP_Compass/AP_Compass_UAVCAN.h

@@ -24,7 +24,6 @@ private:
     int      _mag_fd;
     Vector3f _sum;
     uint32_t _count;
-    uint64_t _last_timestamp;
 
     bool _initialized;
     uint8_t _manager;

libraries/AP_Compass/AP_Compass_qflight.cpp

@@ -80,7 +80,6 @@ void AP_Compass_QFLIGHT::timer_update(void)
     if (ret != 0) {
         return;
     }
-    uint64_t time_us = AP_HAL::micros64();
     for (uint16_t i=0; i<magbuf->num_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);