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

AP_Baro: use millis/micros/panic functions

Browse Source
This commit is contained in:
Caio Marcelo de Oliveira Filho 2015-11-20 12:07:59 +09:00 committed by Randy Mackay
parent b8f0beab83
commit 5a280838f8
4 changed files with 28 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AP_Baro/AP_Baro.cpp
View File

@ -100,11 +100,11 @@ void AP_Baro::calibrate()
// the MS5611 reads quite a long way off for the first second, // the MS5611 reads quite a long way off for the first second,
// leading to about 1m of error if we don't wait // leading to about 1m of error if we don't wait
for (uint8_t i = 0; i < 10; i++) { for (uint8_t i = 0; i < 10; i++) {
uint32_t tstart = hal.scheduler->millis(); uint32_t tstart = AP_HAL::millis();
do { do {
update(); update();
if (hal.scheduler->millis() - tstart > 500) { if (AP_HAL::millis() - tstart > 500) {
hal.scheduler->panic("PANIC: AP_Baro::read unsuccessful " AP_HAL::panic("PANIC: AP_Baro::read unsuccessful "
"for more than 500ms in AP_Baro::calibrate [2]\r\n"); "for more than 500ms in AP_Baro::calibrate [2]\r\n");
} }
hal.scheduler->delay(10); hal.scheduler->delay(10);
@ -120,11 +120,11 @@ void AP_Baro::calibrate()
const uint8_t num_samples = 5; const uint8_t num_samples = 5;
for (uint8_t c = 0; c < num_samples; c++) { for (uint8_t c = 0; c < num_samples; c++) {
uint32_t tstart = hal.scheduler->millis(); uint32_t tstart = AP_HAL::millis();
do { do {
update(); update();
if (hal.scheduler->millis() - tstart > 500) { if (AP_HAL::millis() - tstart > 500) {
hal.scheduler->panic("PANIC: AP_Baro::read unsuccessful " AP_HAL::panic("PANIC: AP_Baro::read unsuccessful "
"for more than 500ms in AP_Baro::calibrate [3]\r\n"); "for more than 500ms in AP_Baro::calibrate [3]\r\n");
} }
} while (!healthy()); } while (!healthy());
@ -152,7 +152,7 @@ void AP_Baro::calibrate()
return; 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)); sensors[i].ground_temperature.set(get_calibration_temperature(i));
// don't notify the GCS too rapidly or we flood the link // 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) { if (now - _last_notify_ms > 10000) {
sensors[i].ground_pressure.notify(); sensors[i].ground_pressure.notify();
sensors[i].ground_temperature.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) void AP_Baro::set_external_temperature(float temperature)
{ {
_external_temperature = 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 float AP_Baro::get_calibration_temperature(uint8_t instance) const
{ {
// if we have a recent external temperature then use it // 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; return _external_temperature;
} }
// if we don't have an external temperature then use the minimum // if we don't have an external temperature then use the minimum
@ -324,7 +324,7 @@ void AP_Baro::init(void)
} }
#endif #endif
if (_num_drivers == 0 || _num_sensors == 0 || drivers[0] == NULL) { 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 // consider a sensor as healthy if it has had an update in the
// last 0.5 seconds // 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++) { for (uint8_t i=0; i<_num_sensors; i++) {
sensors[i].healthy = (now - sensors[i].last_update_ms < 500) && !is_zero(sensors[i].pressure); 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) uint8_t AP_Baro::register_sensor(void)
{ {
if (_num_sensors >= BARO_MAX_INSTANCES) { if (_num_sensors >= BARO_MAX_INSTANCES) {
hal.scheduler->panic("Too many barometers"); AP_HAL::panic("Too many barometers");
} }
return _num_sensors++; return _num_sensors++;
} }

12
libraries/AP_Baro/AP_Baro_BMP085.cpp
View File

@ -35,7 +35,7 @@ extern const AP_HAL::HAL& hal;
// Temp conversion time is 4.5ms // Temp conversion time is 4.5ms
// Pressure conversion time is 25.5ms (for OVERSAMPLING=3) // Pressure conversion time is 25.5ms (for OVERSAMPLING=3)
#define BMP085_EOC -1 #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 #else
#define BMP_DATA_READY() hal.gpio->read(BMP085_EOC) #define BMP_DATA_READY() hal.gpio->read(BMP085_EOC)
#endif #endif
@ -64,7 +64,7 @@ AP_Baro_BMP085::AP_Baro_BMP085(AP_Baro &baro) :
// take i2c bus sempahore // take i2c bus sempahore
if (!i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { 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 // End Of Conversion (PC7) input
@ -74,7 +74,7 @@ AP_Baro_BMP085::AP_Baro_BMP085(AP_Baro &baro) :
// We read the calibration data registers // We read the calibration data registers
if (hal.i2c->readRegisters(BMP085_ADDRESS, 0xAA, 22, buff) != 0) { 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]; 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 // Mode 0x34+(OVERSAMPLING << 6) is osrs=3 when OVERSAMPLING=3 => 25.5ms conversion time
hal.i2c->writeRegister(BMP085_ADDRESS, 0xF4, hal.i2c->writeRegister(BMP085_ADDRESS, 0xF4,
0x34+(OVERSAMPLING << 6)); 0x34+(OVERSAMPLING << 6));
_last_press_read_command_time = hal.scheduler->millis(); _last_press_read_command_time = AP_HAL::millis();
} }
// Read Raw Pressure values // Read Raw Pressure values
@ -173,7 +173,7 @@ bool AP_Baro_BMP085::ReadPress()
uint8_t buf[3]; uint8_t buf[3];
if (hal.i2c->readRegisters(BMP085_ADDRESS, 0xF6, 3, buf) != 0) { 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); hal.i2c->setHighSpeed(false);
return false; return false;
} }
@ -188,7 +188,7 @@ bool AP_Baro_BMP085::ReadPress()
void AP_Baro_BMP085::Command_ReadTemp() void AP_Baro_BMP085::Command_ReadTemp()
{ {
hal.i2c->writeRegister(BMP085_ADDRESS, 0xF4, 0x2E); 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 // Read Raw Temperature values

2
libraries/AP_Baro/AP_Baro_Backend.cpp
View File

@ -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].pressure = pressure;
_frontend.sensors[instance].temperature = temperature; _frontend.sensors[instance].temperature = temperature;
_frontend.sensors[instance].last_update_ms = hal.scheduler->millis(); _frontend.sensors[instance].last_update_ms = AP_HAL::millis();
} }

16
libraries/AP_Baro/AP_Baro_MS5611.cpp
View File

@ -69,11 +69,11 @@ void AP_SerialBus_SPI::init()
{ {
_spi = hal.spi->device(_device); _spi = hal.spi->device(_device);
if (_spi == NULL) { 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(); _spi_sem = _spi->get_semaphore();
if (_spi_sem == NULL) { 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); _spi->set_bus_speed(_speed);
} }
@ -129,7 +129,7 @@ void AP_SerialBus_I2C::init()
{ {
_i2c_sem = _i2c->get_semaphore(); _i2c_sem = _i2c->get_semaphore();
if (_i2c_sem == NULL) { 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(); hal.scheduler->suspend_timer_procs();
if (!_serial->sem_take_blocking()){ 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); _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); _C6 = _serial->read_16bits(CMD_MS5611_PROM_C6);
if (!_check_crc()) { if (!_check_crc()) {
hal.scheduler->panic("Bad CRC on MS5611"); AP_HAL::panic("Bad CRC on MS5611");
} }
// Send a command to read Temp first // Send a command to read Temp first
_serial->write(ADDR_CMD_CONVERT_D2); _serial->write(ADDR_CMD_CONVERT_D2);
_last_timer = hal.scheduler->micros(); _last_timer = AP_HAL::micros();
_state = 0; _state = 0;
_s_D1 = 0; _s_D1 = 0;
@ -286,7 +286,7 @@ bool AP_Baro_MS56XX::_check_crc(void)
void AP_Baro_MS56XX::_timer(void) void AP_Baro_MS56XX::_timer(void)
{ {
// Throttle read rate to 100hz maximum. // Throttle read rate to 100hz maximum.
if (hal.scheduler->micros() - _last_timer < 10000) { if (AP_HAL::micros() - _last_timer < 10000) {
return; return;
} }
@ -349,7 +349,7 @@ void AP_Baro_MS56XX::_timer(void)
} }
} }
_last_timer = hal.scheduler->micros(); _last_timer = AP_HAL::micros();
_serial->sem_give(); _serial->sem_give();
} }