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

AP_Compass: use millis/micros/panic functions

Browse Source
This commit is contained in:
Caio Marcelo de Oliveira Filho 2015-11-20 12:09:17 +09:00 committed by Randy Mackay
parent 89d6acaeaa
commit 0d3fbbdd37
6 changed files with 26 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_Compass/AP_Compass_AK8963.cpp
View File

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

6
libraries/AP_Compass/AP_Compass_Backend.cpp
View File

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

2
libraries/AP_Compass/AP_Compass_HIL.cpp
View File

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

12
libraries/AP_Compass/AP_Compass_HMC5843.cpp
View File

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

14
libraries/AP_Compass/AP_Compass_LSM303D.cpp
View File

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

8
libraries/AP_Compass/Compass.cpp
View File

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