diff --git a/libraries/AP_Compass/AP_Compass_AK8963.cpp b/libraries/AP_Compass/AP_Compass_AK8963.cpp index 567c0d294e..0ab59ef829 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.cpp +++ b/libraries/AP_Compass/AP_Compass_AK8963.cpp @@ -228,10 +228,10 @@ void AP_Compass_AK8963::_update() float mag_x, mag_y, mag_z; // get raw_field - sensor frame, uncorrected Vector3f raw_field; - uint32_t time_us = hal.scheduler->micros(); + uint32_t time_us = AP_HAL::micros(); - if (hal.scheduler->micros() - _last_update_timestamp < 10000) { + if (AP_HAL::micros() - _last_update_timestamp < 10000) { goto end; } @@ -284,7 +284,7 @@ void AP_Compass_AK8963::_update() _accum_count = 5; } - _last_update_timestamp = hal.scheduler->micros(); + _last_update_timestamp = AP_HAL::micros(); fail: _sem_give(); end: @@ -356,7 +356,7 @@ bool AP_Compass_AK8963::_sem_take_nonblocking() if (!hal.scheduler->system_initializing() ) { _sem_failure_count++; if (_sem_failure_count > 100) { - hal.scheduler->panic("PANIC: failed to take _bus->sem " + AP_HAL::panic("PANIC: failed to take _bus->sem " "100 times in a row, in " "AP_Compass_AK8963"); } @@ -393,7 +393,7 @@ AP_AK8963_SerialBus_MPU9250::AP_AK8963_SerialBus_MPU9250(AP_InertialSensor &ins, // getting the semaphore _bus = ins.get_auxiliary_bus(HAL_INS_MPU9250, mpu9250_instance); if (!_bus) - hal.scheduler->panic("Cannot get MPU9250 auxiliary bus"); + AP_HAL::panic("Cannot get MPU9250 auxiliary bus"); _slave = _bus->request_next_slave(addr); } diff --git a/libraries/AP_Compass/AP_Compass_Backend.cpp b/libraries/AP_Compass/AP_Compass_Backend.cpp index 4fc29279d5..1e937c45a1 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.cpp +++ b/libraries/AP_Compass/AP_Compass_Backend.cpp @@ -43,7 +43,7 @@ void AP_Compass_Backend::publish_raw_field(const Vector3f &mag, uint32_t time_us { Compass::mag_state &state = _compass._state[instance]; - state.last_update_ms = hal.scheduler->millis(); + state.last_update_ms = AP_HAL::millis(); // note that we do not set last_update_usec here as otherwise the // EKF and DCM would end up consuming compass data at the full @@ -110,8 +110,8 @@ void AP_Compass_Backend::publish_filtered_field(const Vector3f &mag, uint8_t ins state.field = mag; - state.last_update_ms = hal.scheduler->millis(); - state.last_update_usec = hal.scheduler->micros(); + state.last_update_ms = AP_HAL::millis(); + state.last_update_usec = AP_HAL::micros(); state.has_raw_field = state.updated_raw_field; state.updated_raw_field = false; diff --git a/libraries/AP_Compass/AP_Compass_HIL.cpp b/libraries/AP_Compass/AP_Compass_HIL.cpp index 671fcf2b56..3b76ba2ea7 100644 --- a/libraries/AP_Compass/AP_Compass_HIL.cpp +++ b/libraries/AP_Compass/AP_Compass_HIL.cpp @@ -64,7 +64,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, hal.scheduler->micros(), compass_instance); + publish_raw_field(field, AP_HAL::micros(), compass_instance); correct_field(field, 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 19f8dc4260..4ec375fe07 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.cpp +++ b/libraries/AP_Compass/AP_Compass_HMC5843.cpp @@ -119,7 +119,7 @@ AP_Compass_Backend *AP_Compass_HMC5843::_detect(Compass &compass, bool AP_Compass_HMC5843::read_register(uint8_t address, uint8_t *value) { if (_bus->register_read(address, value) != 0) { - _retry_time = hal.scheduler->millis() + 1000; + _retry_time = AP_HAL::millis() + 1000; return false; } return true; @@ -129,7 +129,7 @@ bool AP_Compass_HMC5843::read_register(uint8_t address, uint8_t *value) bool AP_Compass_HMC5843::write_register(uint8_t address, uint8_t value) { if (_bus->register_write(address, value) != 0) { - _retry_time = hal.scheduler->millis() + 1000; + _retry_time = AP_HAL::millis() + 1000; return false; } return true; @@ -142,7 +142,7 @@ bool AP_Compass_HMC5843::read_raw() if (_bus->read_raw(&rv) != 0) { _bus->set_high_speed(false); - _retry_time = hal.scheduler->millis() + 1000; + _retry_time = AP_HAL::millis() + 1000; return false; } @@ -178,7 +178,7 @@ void AP_Compass_HMC5843::accumulate(void) return; } - uint32_t tnow = hal.scheduler->micros(); + uint32_t tnow = AP_HAL::micros(); if (_accum_count != 0 && (tnow - _last_accum_time) < 13333) { // the compass gets new data at 75Hz return; @@ -439,11 +439,11 @@ void AP_Compass_HMC5843::read() return; } if (_retry_time != 0) { - if (hal.scheduler->millis() < _retry_time) { + if (AP_HAL::millis() < _retry_time) { return; } if (!re_initialise()) { - _retry_time = hal.scheduler->millis() + 1000; + _retry_time = AP_HAL::millis() + 1000; _bus->set_high_speed(false); return; } diff --git a/libraries/AP_Compass/AP_Compass_LSM303D.cpp b/libraries/AP_Compass/AP_Compass_LSM303D.cpp index 3e52709ce7..681bbea134 100644 --- a/libraries/AP_Compass/AP_Compass_LSM303D.cpp +++ b/libraries/AP_Compass/AP_Compass_LSM303D.cpp @@ -302,7 +302,7 @@ AP_Compass_LSM303D::init() uint8_t whoami = _register_read(ADDR_WHO_AM_I); if (whoami != WHO_I_AM) { hal.console->printf("LSM303D: unexpected WHOAMI 0x%x\n", (unsigned)whoami); - hal.scheduler->panic("LSM303D: bad WHOAMI"); + AP_HAL::panic("LSM303D: bad WHOAMI"); } uint8_t tries = 0; @@ -312,7 +312,7 @@ AP_Compass_LSM303D::init() if (success) { hal.scheduler->delay(5+2); if (!_spi_sem->take(100)) { - hal.scheduler->panic("LSM303D: Unable to get semaphore"); + AP_HAL::panic("LSM303D: Unable to get semaphore"); } if (_data_ready()) { _spi_sem->give(); @@ -324,7 +324,7 @@ AP_Compass_LSM303D::init() _spi_sem->give(); } if (tries++ > 5) { - hal.scheduler->panic("PANIC: failed to boot LSM303D 5 times"); + AP_HAL::panic("PANIC: failed to boot LSM303D 5 times"); } } while (1); @@ -356,7 +356,7 @@ uint32_t AP_Compass_LSM303D::get_dev_id() bool AP_Compass_LSM303D::_hardware_init(void) { if (!_spi_sem->take(100)) { - hal.scheduler->panic("LSM303D: Unable to get semaphore"); + AP_HAL::panic("LSM303D: Unable to get semaphore"); } // initially run the bus at low speed @@ -385,7 +385,7 @@ bool AP_Compass_LSM303D::_hardware_init(void) void AP_Compass_LSM303D::_update() { - if (hal.scheduler->micros() - _last_update_timestamp < 10000) { + if (AP_HAL::micros() - _last_update_timestamp < 10000) { return; } @@ -395,7 +395,7 @@ void AP_Compass_LSM303D::_update() _collect_samples(); - _last_update_timestamp = hal.scheduler->micros(); + _last_update_timestamp = AP_HAL::micros(); _spi_sem->give(); } @@ -409,7 +409,7 @@ void AP_Compass_LSM303D::_collect_samples() error("_read_raw() failed\n"); } else { Vector3f raw_field = Vector3f(_mag_x, _mag_y, _mag_z) * _mag_range_scale; - uint32_t time_us = hal.scheduler->micros(); + uint32_t time_us = AP_HAL::micros(); // rotate raw_field from sensor frame to body frame rotate_field(raw_field, _compass_instance); diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index f9ab928829..30181a221f 100644 --- a/libraries/AP_Compass/Compass.cpp +++ b/libraries/AP_Compass/Compass.cpp @@ -400,7 +400,7 @@ Compass::init() uint8_t Compass::register_compass(void) { if (_compass_count == COMPASS_MAX_INSTANCES) { - hal.scheduler->panic("Too many compass instances"); + AP_HAL::panic("Too many compass instances"); } return _compass_count++; } @@ -410,7 +410,7 @@ void Compass::_add_backend(AP_Compass_Backend *backend) if (!backend) return; if (_backend_count == COMPASS_MAX_BACKEND) - hal.scheduler->panic("Too many compass backends"); + AP_HAL::panic("Too many compass backends"); _backends[_backend_count++] = backend; } @@ -476,7 +476,7 @@ Compass::read(void) _backends[i]->read(); } for (uint8_t i=0; i < COMPASS_MAX_INSTANCES; i++) { - _state[i].healthy = (hal.scheduler->millis() - _state[i].last_update_ms < 500); + _state[i].healthy = (AP_HAL::millis() - _state[i].last_update_ms < 500); } return healthy(); } @@ -724,7 +724,7 @@ void Compass::setHIL(uint8_t instance, const Vector3f &mag) { _hil.field[instance] = mag; _hil.healthy[instance] = true; - _state[instance].last_update_usec = hal.scheduler->micros(); + _state[instance].last_update_usec = AP_HAL::micros(); } const Vector3f& Compass::getHIL(uint8_t instance) const