Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-07 00:13:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Baro: use WITH_SEMAPHORE()

Browse Source 
and removed usage of hal.util->new_semaphore()
This commit is contained in:
Andrew Tridgell 2018-10-12 10:35:03 +11:00
parent fe064a2d4e
commit e4e793b295
10 changed files with 69 additions and 104 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AP_Baro/AP_Baro_BMP085.cpp
View File

@ -66,9 +66,7 @@ bool AP_Baro_BMP085::_init()
AP_HAL::Semaphore *sem = _dev->get_semaphore();
// take i2c bus sempahore
if (!sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
AP_HAL::panic("BMP085: unable to get semaphore");
}
WITH_SEMAPHORE(sem);
if (BMP085_EOC >= 0) {
_eoc = hal.gpio->channel(BMP085_EOC);
@ -79,7 +77,6 @@ bool AP_Baro_BMP085::_init()
uint8_t id;
if (!_dev->read_registers(0xD0, &id, 1)) {
sem->give();
return false;
}
@ -106,7 +103,6 @@ bool AP_Baro_BMP085::_init()
}
}
if (!prom_ok) {
sem->give();
return false;
}
@ -141,8 +137,6 @@ bool AP_Baro_BMP085::_init()
_instance = _frontend.register_sensor();
sem->give();
_dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&AP_Baro_BMP085::_timer, void));
return true;
}
@ -203,18 +197,16 @@ void AP_Baro_BMP085::_timer(void)
*/
void AP_Baro_BMP085::update(void)
{
if (_sem.take_nonblocking()) {
if (!_has_sample) {
_sem.give();
return;
}
WITH_SEMAPHORE(_sem);
float temperature = 0.1f * _temp;
float pressure = _pressure_filter.getf();
_copy_to_frontend(_instance, pressure, temperature);
_sem.give();
if (!_has_sample) {
return;
}
float temperature = 0.1f * _temp;
float pressure = _pressure_filter.getf();
_copy_to_frontend(_instance, pressure, temperature);
}
// Send command to Read Pressure

20
libraries/AP_Baro/AP_Baro_BMP280.cpp
View File

@ -66,9 +66,10 @@ AP_Baro_Backend *AP_Baro_BMP280::probe(AP_Baro &baro,
bool AP_Baro_BMP280::_init()
{
if (!_dev | !_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
if (!_dev) {
return false;
}
WITH_SEMAPHORE(_dev->get_semaphore());
_has_sample = false;
@ -78,7 +79,6 @@ bool AP_Baro_BMP280::_init()
if (!_dev->read_registers(BMP280_REG_ID, &whoami, 1) ||
whoami != BMP280_ID) {
// not a BMP280
_dev->get_semaphore()->give();
return false;
}
@ -114,8 +114,6 @@ bool AP_Baro_BMP280::_init()
_instance = _frontend.register_sensor();
_dev->get_semaphore()->give();
// request 50Hz update
_dev->register_periodic_callback(20 * AP_USEC_PER_MSEC, FUNCTOR_BIND_MEMBER(&AP_Baro_BMP280::_timer, void));
@ -140,16 +138,14 @@ void AP_Baro_BMP280::_timer(void)
// transfer data to the frontend
void AP_Baro_BMP280::update(void)
{
if (_sem.take_nonblocking()) {
if (!_has_sample) {
_sem.give();
return;
}
WITH_SEMAPHORE(_sem);
_copy_to_frontend(_instance, _pressure, _temperature);
_has_sample = false;
_sem.give();
if (!_has_sample) {
return;
}
_copy_to_frontend(_instance, _pressure, _temperature);
_has_sample = false;
}
// calculate temperature

6
libraries/AP_Baro/AP_Baro_Backend.cpp
View File

@ -14,9 +14,7 @@ void AP_Baro_Backend::update_healthy_flag(uint8_t instance)
if (instance >= _frontend._num_sensors) {
return;
}
if (!_sem.take_nonblocking()) {
return;
}
WITH_SEMAPHORE(_sem);
// consider a sensor as healthy if it has had an update in the
// last 0.5 seconds and values are non-zero and have changed within the last 2 seconds
@ -25,8 +23,6 @@ void AP_Baro_Backend::update_healthy_flag(uint8_t instance)
(now - _frontend.sensors[instance].last_update_ms < BARO_TIMEOUT_MS) &&
(now - _frontend.sensors[instance].last_change_ms < BARO_DATA_CHANGE_TIMEOUT_MS) &&
!is_zero(_frontend.sensors[instance].pressure);
_sem.give();
}
void AP_Baro_Backend::backend_update(uint8_t instance)

2
libraries/AP_Baro/AP_Baro_Backend.h
View File

@ -31,7 +31,7 @@ protected:
void _copy_to_frontend(uint8_t instance, float pressure, float temperature);
// semaphore for access to shared frontend data
HAL_Semaphore _sem;
HAL_Semaphore_Recursive _sem;
virtual void update_healthy_flag(uint8_t instance);

32
libraries/AP_Baro/AP_Baro_DPS280.cpp
View File

@ -210,28 +210,24 @@ void AP_Baro_DPS280::timer(void)
return;
}
if (_sem.take_nonblocking()) {
pressure_sum += pressure;
temperature_sum += temperature;
count++;
_sem.give();
}
WITH_SEMAPHORE(_sem);
pressure_sum += pressure;
temperature_sum += temperature;
count++;
}
// transfer data to the frontend
void AP_Baro_DPS280::update(void)
{
if (count != 0 && _sem.take_nonblocking()) {
if (count == 0) {
_sem.give();
return;
}
_copy_to_frontend(instance, pressure_sum/count, temperature_sum/count);
pressure_sum = 0;
temperature_sum = 0;
count=0;
_sem.give();
if (count == 0) {
return;
}
WITH_SEMAPHORE(_sem);
_copy_to_frontend(instance, pressure_sum/count, temperature_sum/count);
pressure_sum = 0;
temperature_sum = 0;
count=0;
}

24
libraries/AP_Baro/AP_Baro_FBM320.cpp
View File

@ -187,12 +187,12 @@ void AP_Baro_FBM320::timer(void)
} else {
int32_t pressure, temperature;
calculate_PT(value_T, value, pressure, temperature);
if (pressure_ok(pressure) && _sem.take_nonblocking()) {
if (pressure_ok(pressure)) {
WITH_SEMAPHORE(_sem);
pressure_sum += pressure;
// sum and convert to degrees
temperature_sum += temperature*0.01;
count++;
_sem.give();
}
}
@ -207,17 +207,13 @@ void AP_Baro_FBM320::timer(void)
// transfer data to the frontend
void AP_Baro_FBM320::update(void)
{
if (count != 0 && _sem.take_nonblocking()) {
if (count == 0) {
_sem.give();
return;
}
_copy_to_frontend(instance, pressure_sum/count, temperature_sum/count);
pressure_sum = 0;
temperature_sum = 0;
count=0;
_sem.give();
if (count == 0) {
return;
}
WITH_SEMAPHORE(_sem);
_copy_to_frontend(instance, pressure_sum/count, temperature_sum/count);
pressure_sum = 0;
temperature_sum = 0;
count=0;
}

37
libraries/AP_Baro/AP_Baro_LPS2XH.cpp
View File

@ -205,16 +205,13 @@ void AP_Baro_LPS2XH::_timer(void)
// transfer data to the frontend
void AP_Baro_LPS2XH::update(void)
{
if (_sem.take_nonblocking()) {
if (!_has_sample) {
_sem.give();
return;
}
_copy_to_frontend(_instance, _pressure, _temperature);
_has_sample = false;
_sem.give();
if (!_has_sample) {
return;
}
WITH_SEMAPHORE(_sem);
_copy_to_frontend(_instance, _pressure, _temperature);
_has_sample = false;
}
// calculate temperature
@ -224,14 +221,13 @@ void AP_Baro_LPS2XH::_update_temperature(void)
_dev->read_registers(TEMP_OUT_ADDR, pu8, 2);
int16_t Temp_Reg_s16 = (uint16_t)(pu8[1]<<8) | pu8[0];
if (_sem.take_nonblocking()) {
if(_lps2xh_type == BARO_LPS25H){
_temperature=((float)(Temp_Reg_s16/480)+42.5);
}
if(_lps2xh_type == BARO_LPS22H){
_temperature=(float)(Temp_Reg_s16/100);
}
_sem.give();
WITH_SEMAPHORE(_sem);
if (_lps2xh_type == BARO_LPS25H) {
_temperature=((float)(Temp_Reg_s16/480)+42.5);
}
if (_lps2xh_type == BARO_LPS22H) {
_temperature=(float)(Temp_Reg_s16/100);
}
}
@ -242,8 +238,7 @@ void AP_Baro_LPS2XH::_update_pressure(void)
_dev->read_registers(PRESS_OUT_XL_ADDR, pressure, 3);
int32_t Pressure_Reg_s32 = ((uint32_t)pressure[2]<<16)|((uint32_t)pressure[1]<<8)|(uint32_t)pressure[0];
int32_t Pressure_mb = Pressure_Reg_s32 * (100.0 / 4096); // scale for pa
if (_sem.take_nonblocking()) {
_pressure=Pressure_mb;
_sem.give();
}
WITH_SEMAPHORE(_sem);
_pressure = Pressure_mb;
}

15
libraries/AP_Baro/AP_Baro_SITL.cpp
View File

@ -121,16 +121,13 @@ void AP_Baro_SITL::_timer()
// Read the sensor
void AP_Baro_SITL::update(void)
{
if (_sem.take_nonblocking()) {
if (!_has_sample) {
_sem.give();
return;
}
_copy_to_frontend(_instance, _recent_press, _recent_temp);
_has_sample = false;
_sem.give();
if (!_has_sample) {
return;
}
WITH_SEMAPHORE(_sem);
_copy_to_frontend(_instance, _recent_press, _recent_temp);
_has_sample = false;
}
#endif // CONFIG_HAL_BOARD

9
libraries/AP_Baro/AP_Baro_UAVCAN.cpp
View File

@ -19,7 +19,7 @@ UC_REGISTRY_BINDER(PressureCb, uavcan::equipment::air_data::StaticPressure);
UC_REGISTRY_BINDER(TemperatureCb, uavcan::equipment::air_data::StaticTemperature);
AP_Baro_UAVCAN::DetectedModules AP_Baro_UAVCAN::_detected_modules[] = {0};
AP_HAL::Semaphore* AP_Baro_UAVCAN::_sem_registry = nullptr;
HAL_Semaphore AP_Baro_UAVCAN::_sem_registry;
/*
constructor - registers instance at top Baro driver
@ -57,15 +57,12 @@ void AP_Baro_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan)
bool AP_Baro_UAVCAN::take_registry()
{
if (_sem_registry == nullptr) {
_sem_registry = hal.util->new_semaphore();
}
return _sem_registry->take(HAL_SEMAPHORE_BLOCK_FOREVER);
return _sem_registry.take(HAL_SEMAPHORE_BLOCK_FOREVER);
}
void AP_Baro_UAVCAN::give_registry()
{
_sem_registry->give();
_sem_registry.give();
}
AP_Baro_Backend* AP_Baro_UAVCAN::probe(AP_Baro &baro)

2
libraries/AP_Baro/AP_Baro_UAVCAN.h
View File

@ -47,5 +47,5 @@ private:
AP_Baro_UAVCAN* driver;
} _detected_modules[BARO_MAX_DRIVERS];
static AP_HAL::Semaphore *_sem_registry;
static HAL_Semaphore _sem_registry;
};