diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index 53a8743c21..aa30482eeb 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -100,11 +100,11 @@ void AP_Baro::calibrate() // the MS5611 reads quite a long way off for the first second, // leading to about 1m of error if we don't wait for (uint8_t i = 0; i < 10; i++) { - uint32_t tstart = hal.scheduler->millis(); + uint32_t tstart = AP_HAL::millis(); do { update(); - if (hal.scheduler->millis() - tstart > 500) { - hal.scheduler->panic("PANIC: AP_Baro::read unsuccessful " + if (AP_HAL::millis() - tstart > 500) { + AP_HAL::panic("PANIC: AP_Baro::read unsuccessful " "for more than 500ms in AP_Baro::calibrate [2]\r\n"); } hal.scheduler->delay(10); @@ -120,11 +120,11 @@ void AP_Baro::calibrate() const uint8_t num_samples = 5; for (uint8_t c = 0; c < num_samples; c++) { - uint32_t tstart = hal.scheduler->millis(); + uint32_t tstart = AP_HAL::millis(); do { update(); - if (hal.scheduler->millis() - tstart > 500) { - hal.scheduler->panic("PANIC: AP_Baro::read unsuccessful " + if (AP_HAL::millis() - tstart > 500) { + AP_HAL::panic("PANIC: AP_Baro::read unsuccessful " "for more than 500ms in AP_Baro::calibrate [3]\r\n"); } } while (!healthy()); @@ -152,7 +152,7 @@ void AP_Baro::calibrate() return; } } - hal.scheduler->panic("AP_Baro: all sensors uncalibrated"); + AP_HAL::panic("AP_Baro: all sensors uncalibrated"); } /* @@ -170,7 +170,7 @@ void AP_Baro::update_calibration() sensors[i].ground_temperature.set(get_calibration_temperature(i)); // don't notify the GCS too rapidly or we flood the link - uint32_t now = hal.scheduler->millis(); + uint32_t now = AP_HAL::millis(); if (now - _last_notify_ms > 10000) { sensors[i].ground_pressure.notify(); sensors[i].ground_temperature.notify(); @@ -245,7 +245,7 @@ float AP_Baro::get_climb_rate(void) void AP_Baro::set_external_temperature(float temperature) { _external_temperature = temperature; - _last_external_temperature_ms = hal.scheduler->millis(); + _last_external_temperature_ms = AP_HAL::millis(); } /* @@ -254,7 +254,7 @@ void AP_Baro::set_external_temperature(float temperature) float AP_Baro::get_calibration_temperature(uint8_t instance) const { // if we have a recent external temperature then use it - if (_last_external_temperature_ms != 0 && hal.scheduler->millis() - _last_external_temperature_ms < 10000) { + if (_last_external_temperature_ms != 0 && AP_HAL::millis() - _last_external_temperature_ms < 10000) { return _external_temperature; } // if we don't have an external temperature then use the minimum @@ -324,7 +324,7 @@ void AP_Baro::init(void) } #endif if (_num_drivers == 0 || _num_sensors == 0 || drivers[0] == NULL) { - hal.scheduler->panic("Baro: unable to initialise driver"); + AP_HAL::panic("Baro: unable to initialise driver"); } } @@ -342,7 +342,7 @@ void AP_Baro::update(void) // consider a sensor as healthy if it has had an update in the // last 0.5 seconds - uint32_t now = hal.scheduler->millis(); + uint32_t now = AP_HAL::millis(); for (uint8_t i=0; i<_num_sensors; i++) { sensors[i].healthy = (now - sensors[i].last_update_ms < 500) && !is_zero(sensors[i].pressure); } @@ -398,7 +398,7 @@ void AP_Baro::accumulate(void) uint8_t AP_Baro::register_sensor(void) { if (_num_sensors >= BARO_MAX_INSTANCES) { - hal.scheduler->panic("Too many barometers"); + AP_HAL::panic("Too many barometers"); } return _num_sensors++; } diff --git a/libraries/AP_Baro/AP_Baro_BMP085.cpp b/libraries/AP_Baro/AP_Baro_BMP085.cpp index 502d4801b7..8343bfcb65 100644 --- a/libraries/AP_Baro/AP_Baro_BMP085.cpp +++ b/libraries/AP_Baro/AP_Baro_BMP085.cpp @@ -35,7 +35,7 @@ extern const AP_HAL::HAL& hal; // Temp conversion time is 4.5ms // Pressure conversion time is 25.5ms (for OVERSAMPLING=3) #define BMP085_EOC -1 -#define BMP_DATA_READY() (BMP085_State == 0 ? hal.scheduler->millis() > (_last_temp_read_command_time + 5) : hal.scheduler->millis() > (_last_press_read_command_time + 26)) +#define BMP_DATA_READY() (BMP085_State == 0 ? AP_HAL::millis() > (_last_temp_read_command_time + 5) : AP_HAL::millis() > (_last_press_read_command_time + 26)) #else #define BMP_DATA_READY() hal.gpio->read(BMP085_EOC) #endif @@ -64,7 +64,7 @@ AP_Baro_BMP085::AP_Baro_BMP085(AP_Baro &baro) : // take i2c bus sempahore if (!i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - hal.scheduler->panic("BMP085: unable to get semaphore"); + AP_HAL::panic("BMP085: unable to get semaphore"); } // End Of Conversion (PC7) input @@ -74,7 +74,7 @@ AP_Baro_BMP085::AP_Baro_BMP085(AP_Baro &baro) : // We read the calibration data registers if (hal.i2c->readRegisters(BMP085_ADDRESS, 0xAA, 22, buff) != 0) { - hal.scheduler->panic("BMP085: bad calibration registers"); + AP_HAL::panic("BMP085: bad calibration registers"); } ac1 = ((int16_t)buff[0] << 8) | buff[1]; @@ -164,7 +164,7 @@ void AP_Baro_BMP085::Command_ReadPress() // Mode 0x34+(OVERSAMPLING << 6) is osrs=3 when OVERSAMPLING=3 => 25.5ms conversion time hal.i2c->writeRegister(BMP085_ADDRESS, 0xF4, 0x34+(OVERSAMPLING << 6)); - _last_press_read_command_time = hal.scheduler->millis(); + _last_press_read_command_time = AP_HAL::millis(); } // Read Raw Pressure values @@ -173,7 +173,7 @@ bool AP_Baro_BMP085::ReadPress() uint8_t buf[3]; if (hal.i2c->readRegisters(BMP085_ADDRESS, 0xF6, 3, buf) != 0) { - _retry_time = hal.scheduler->millis() + 1000; + _retry_time = AP_HAL::millis() + 1000; hal.i2c->setHighSpeed(false); return false; } @@ -188,7 +188,7 @@ bool AP_Baro_BMP085::ReadPress() void AP_Baro_BMP085::Command_ReadTemp() { hal.i2c->writeRegister(BMP085_ADDRESS, 0xF4, 0x2E); - _last_temp_read_command_time = hal.scheduler->millis(); + _last_temp_read_command_time = AP_HAL::millis(); } // Read Raw Temperature values diff --git a/libraries/AP_Baro/AP_Baro_Backend.cpp b/libraries/AP_Baro/AP_Baro_Backend.cpp index 2d165206fd..ae662457b8 100644 --- a/libraries/AP_Baro/AP_Baro_Backend.cpp +++ b/libraries/AP_Baro/AP_Baro_Backend.cpp @@ -19,5 +19,5 @@ void AP_Baro_Backend::_copy_to_frontend(uint8_t instance, float pressure, float } _frontend.sensors[instance].pressure = pressure; _frontend.sensors[instance].temperature = temperature; - _frontend.sensors[instance].last_update_ms = hal.scheduler->millis(); + _frontend.sensors[instance].last_update_ms = AP_HAL::millis(); } diff --git a/libraries/AP_Baro/AP_Baro_MS5611.cpp b/libraries/AP_Baro/AP_Baro_MS5611.cpp index b8e08c5712..543ffc2e1f 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.cpp +++ b/libraries/AP_Baro/AP_Baro_MS5611.cpp @@ -69,11 +69,11 @@ void AP_SerialBus_SPI::init() { _spi = hal.spi->device(_device); if (_spi == NULL) { - hal.scheduler->panic("did not get valid SPI device driver!"); + AP_HAL::panic("did not get valid SPI device driver!"); } _spi_sem = _spi->get_semaphore(); if (_spi_sem == NULL) { - hal.scheduler->panic("AP_SerialBus_SPI did not get valid SPI semaphroe!"); + AP_HAL::panic("AP_SerialBus_SPI did not get valid SPI semaphroe!"); } _spi->set_bus_speed(_speed); } @@ -129,7 +129,7 @@ void AP_SerialBus_I2C::init() { _i2c_sem = _i2c->get_semaphore(); if (_i2c_sem == NULL) { - hal.scheduler->panic("AP_SerialBus_I2C did not get valid I2C semaphore!"); + AP_HAL::panic("AP_SerialBus_I2C did not get valid I2C semaphore!"); } } @@ -192,7 +192,7 @@ AP_Baro_MS56XX::AP_Baro_MS56XX(AP_Baro &baro, AP_SerialBus *serial, bool use_tim hal.scheduler->suspend_timer_procs(); if (!_serial->sem_take_blocking()){ - hal.scheduler->panic("PANIC: AP_Baro_MS56XX: failed to take serial semaphore for init"); + AP_HAL::panic("PANIC: AP_Baro_MS56XX: failed to take serial semaphore for init"); } _serial->write(CMD_MS5611_RESET); @@ -208,12 +208,12 @@ AP_Baro_MS56XX::AP_Baro_MS56XX(AP_Baro &baro, AP_SerialBus *serial, bool use_tim _C6 = _serial->read_16bits(CMD_MS5611_PROM_C6); if (!_check_crc()) { - hal.scheduler->panic("Bad CRC on MS5611"); + AP_HAL::panic("Bad CRC on MS5611"); } // Send a command to read Temp first _serial->write(ADDR_CMD_CONVERT_D2); - _last_timer = hal.scheduler->micros(); + _last_timer = AP_HAL::micros(); _state = 0; _s_D1 = 0; @@ -286,7 +286,7 @@ bool AP_Baro_MS56XX::_check_crc(void) void AP_Baro_MS56XX::_timer(void) { // Throttle read rate to 100hz maximum. - if (hal.scheduler->micros() - _last_timer < 10000) { + if (AP_HAL::micros() - _last_timer < 10000) { return; } @@ -349,7 +349,7 @@ void AP_Baro_MS56XX::_timer(void) } } - _last_timer = hal.scheduler->micros(); + _last_timer = AP_HAL::micros(); _serial->sem_give(); }