aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Michael du Breuil 2017-09-05 18:58:35 -07:00 committed by Francisco Ferreira
parent a88e663191
commit fb77d0739e
20 changed files with 19 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_Compass/AP_Compass.cpp
View File

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

2
libraries/AP_Compass/AP_Compass_AK09916.cpp
View File

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

3
libraries/AP_Compass/AP_Compass_AK8963.cpp
View File

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

4
libraries/AP_Compass/AP_Compass_BMM150.cpp
View File

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

2
libraries/AP_Compass/AP_Compass_Backend.cpp
View File

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

2
libraries/AP_Compass/AP_Compass_Backend.h
View File

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

2
libraries/AP_Compass/AP_Compass_HIL.cpp
View File

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

4
libraries/AP_Compass/AP_Compass_HMC5843.cpp
View File

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

4
libraries/AP_Compass/AP_Compass_IST8310.cpp
View File

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

2
libraries/AP_Compass/AP_Compass_LIS3MDL.cpp
View File

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

2
libraries/AP_Compass/AP_Compass_LSM303D.cpp
View File

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

3
libraries/AP_Compass/AP_Compass_LSM9DS1.cpp
View File

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

2
libraries/AP_Compass/AP_Compass_MMC3416.cpp
View File

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

4
libraries/AP_Compass/AP_Compass_QMC5883L.cpp
View File

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

4
libraries/AP_Compass/AP_Compass_QURT.cpp
View File

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

1
libraries/AP_Compass/AP_Compass_QURT.h
View File

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

2
libraries/AP_Compass/AP_Compass_SITL.cpp
View File

@ -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]);
}

3
libraries/AP_Compass/AP_Compass_UAVCAN.cpp
View File

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

1
libraries/AP_Compass/AP_Compass_UAVCAN.h
View File

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

3
libraries/AP_Compass/AP_Compass_qflight.cpp
View File

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