Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Baro_MS5611: uses new semaphore lib

Browse Source
This commit is contained in:
Pat Hickey 2013-01-03 11:05:00 -08:00
parent a556a95565
commit 5d91f342bb
2 changed files with 49 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
libraries/AP_Baro/AP_Baro_MS5611.cpp
View File

@ -67,11 +67,17 @@ void AP_Baro_MS5611_SPI::init()
{
_spi = hal.spi->device(AP_HAL::SPIDevice_MS5611);
if (_spi == NULL) {
hal.scheduler->panic(PSTR("PANIC: AP_Baro_MS5611 could not get "
hal.scheduler->panic(PSTR("PANIC: AP_Baro_MS5611 did not get "
"valid SPI device driver!"));
return;
return; /* never reached */
}
_spi_sem = _spi->get_semaphore();
if (_spi_sem == NULL) {
hal.scheduler->panic(PSTR("PANIC: AP_Baro_MS5611 did not get "
"valid SPI semaphroe!"));
return; /* never reached */
}
}
uint16_t AP_Baro_MS5611_SPI::read_16bits(uint8_t reg)
@ -100,31 +106,34 @@ void AP_Baro_MS5611_SPI::write(uint8_t reg)
_spi->transaction(tx, NULL, 1);
}
void AP_Baro_MS5611_SPI::sem_get()
bool AP_Baro_MS5611_SPI::sem_take_blocking() {
return _spi_sem->take(10);
}
bool AP_Baro_MS5611_SPI::sem_take_nonblocking()
{
/**
* Take nonblocking from a TimerProcess context &
* monitor for bad failures
*/
static int semfail_ctr = 0;
if (_spi_sem) {
bool got = _spi_sem->get((void*)&_spi_sem);
bool got = _spi_sem->take_nonblocking();
if (!got) {
semfail_ctr++;
if (semfail_ctr > 100) {
hal.scheduler->panic(PSTR("PANIC: failed to take _spi_sem "
"100 times in AP_Baro_MS5611::_update"));
}
return;
"100 times in a row, in AP_Baro_MS5611::_update"));
}
return false; /* never reached */
} else {
semfail_ctr = 0;
}
return got;
}
void AP_Baro_MS5611_SPI::sem_release()
void AP_Baro_MS5611_SPI::sem_give()
{
if (_spi_sem) {
bool released = _spi_sem->release((void*)&_spi_sem);
if (!released) {
hal.scheduler->panic(PSTR("PANIC: _spi_sem release failed in "
"AP_Baro_MS5611::_update"));
}
}
_spi_sem->give();
}
// I2C Device //////////////////////////////////////////////////////////////////
@ -168,11 +177,15 @@ bool AP_Baro_MS5611::init()
{
if (_serial == NULL) {
hal.scheduler->panic(PSTR("PANIC: AP_Baro_MS5611: NULL serial driver"));
return false;
return false; /* never reached */
}
_serial->init();
_serial->sem_get();
if (!_serial->sem_take_blocking()){
hal.scheduler->panic(PSTR("PANIC: AP_Baro_MS5611: failed to take "
"serial semaphore for init"));
return false; /* never reached */
}
_serial->write(CMD_MS5611_RESET);
hal.scheduler->delay(4);
@ -200,7 +213,7 @@ bool AP_Baro_MS5611::init()
_d2_count = 0;
hal.scheduler->register_timer_process( AP_Baro_MS5611::_update );
_serial->sem_release();
_serial->sem_give();
// wait for at least one value to be read
uint32_t tstart = hal.scheduler->millis();
@ -231,7 +244,7 @@ void AP_Baro_MS5611::_update(uint32_t tnow)
return;
}
_serial->sem_get();
_serial->sem_take_nonblocking();
_timer = tnow;
if (_state == 0) {
@ -267,7 +280,7 @@ void AP_Baro_MS5611::_update(uint32_t tnow)
}
}
_serial->sem_release();
_serial->sem_give();
}
uint8_t AP_Baro_MS5611::read()

14
libraries/AP_Baro/AP_Baro_MS5611.h
View File

@ -22,11 +22,14 @@ public:
/** Write a single byte command. */
virtual void write(uint8_t reg) = 0;
/** Acquire the internal semaphore for this device. */
virtual void sem_get() {}
/** Acquire the internal semaphore for this device.
* take_nonblocking should be used from the timer process,
* take_blocking from synchronous code (i.e. init) */
virtual bool sem_take_nonblocking() { return true; }
virtual bool sem_take_blocking() { return true; }
/** Release the internal semaphore for this device. */
virtual void sem_release() {}
virtual void sem_give() {}
};
/** SPI serial device. */
@ -37,8 +40,9 @@ public:
virtual uint16_t read_16bits(uint8_t reg);
virtual uint32_t read_adc();
virtual void write(uint8_t reg);
virtual void sem_get();
virtual void sem_release();
virtual bool sem_take_nonblocking();
virtual bool sem_take_blocking();
virtual void sem_give();
private:
AP_HAL::SPIDeviceDriver *_spi;