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

AP_Airspeed: 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 725899080e
commit fe064a2d4e
8 changed files with 85 additions and 88 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_Airspeed/AP_Airspeed_Backend.cpp
View File

@ -28,12 +28,10 @@ AP_Airspeed_Backend::AP_Airspeed_Backend(AP_Airspeed &_frontend, uint8_t _instan
frontend(_frontend),
instance(_instance)
{
sem = hal.util->new_semaphore();
}
AP_Airspeed_Backend::~AP_Airspeed_Backend(void)
{
delete sem;
}

2
libraries/AP_Airspeed/AP_Airspeed_Backend.h
View File

@ -47,7 +47,7 @@ protected:
}
// semaphore for access to shared frontend data
AP_HAL::Semaphore *sem;
HAL_Semaphore sem;
float get_airspeed_ratio(void) const {
return frontend.get_airspeed_ratio(instance);

25
libraries/AP_Airspeed/AP_Airspeed_DLVR.cpp
View File

@ -82,13 +82,13 @@ void AP_Airspeed_DLVR::timer()
float press_h2o = 1.25f * 2.0f * DLVR_FSS * ((pres_raw - DLVR_OFFSET) / DLVR_SCALE);
float temp = temp_raw * (200.0f / 2047.0f) - 50.0f;
sem->take_blocking();
WITH_SEMAPHORE(sem);
pressure_sum += INCH_OF_H2O_TO_PASCAL * press_h2o;
temperature_sum += temp;
press_count++;
temp_count++;
last_sample_time_ms = AP_HAL::millis();
sem->give();
}
// return the current differential_pressure in Pascal
@ -97,14 +97,16 @@ bool AP_Airspeed_DLVR::get_differential_pressure(float &_pressure)
if ((AP_HAL::millis() - last_sample_time_ms) > 100) {
return false;
}
if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
{
WITH_SEMAPHORE(sem);
if (press_count > 0) {
pressure = pressure_sum / press_count;
press_count = 0;
pressure_sum = 0;
}
sem->give();
}
_pressure = pressure;
return true;
}
@ -115,14 +117,15 @@ bool AP_Airspeed_DLVR::get_temperature(float &_temperature)
if ((AP_HAL::millis() - last_sample_time_ms) > 100) {
return false;
}
if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
if (temp_count > 0) {
temperature = temperature_sum / temp_count;
temp_count = 0;
temperature_sum = 0;
}
sem->give();
WITH_SEMAPHORE(sem);
if (temp_count > 0) {
temperature = temperature_sum / temp_count;
temp_count = 0;
temperature_sum = 0;
}
_temperature = temperature;
return true;
}

51
libraries/AP_Airspeed/AP_Airspeed_MS4525.cpp
View File

@ -51,9 +51,7 @@ bool AP_Airspeed_MS4525::init()
if (!_dev) {
continue;
}
if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
continue;
}
WITH_SEMAPHORE(_dev->get_semaphore());
// lots of retries during probe
_dev->set_retries(10);
@ -62,8 +60,6 @@ bool AP_Airspeed_MS4525::init()
hal.scheduler->delay(10);
_collect();
_dev->get_semaphore()->give();
found = (_last_sample_time_ms != 0);
if (found) {
printf("MS4525: Found sensor on bus %u address 0x%02x\n", addresses[i].bus, addresses[i].addr);
@ -176,14 +172,13 @@ void AP_Airspeed_MS4525::_collect()
_voltage_correction(press, temp);
_voltage_correction(press2, temp2);
if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
_press_sum += press + press2;
_temp_sum += temp + temp2;
_press_count += 2;
_temp_count += 2;
sem->give();
}
WITH_SEMAPHORE(sem);
_press_sum += press + press2;
_temp_sum += temp + temp2;
_press_count += 2;
_temp_count += 2;
_last_sample_time_ms = AP_HAL::millis();
}
@ -230,14 +225,15 @@ bool AP_Airspeed_MS4525::get_differential_pressure(float &pressure)
if ((AP_HAL::millis() - _last_sample_time_ms) > 100) {
return false;
}
if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
if (_press_count > 0) {
_pressure = _press_sum / _press_count;
_press_count = 0;
_press_sum = 0;
}
sem->give();
WITH_SEMAPHORE(sem);
if (_press_count > 0) {
_pressure = _press_sum / _press_count;
_press_count = 0;
_press_sum = 0;
}
pressure = _pressure;
return true;
}
@ -248,14 +244,15 @@ bool AP_Airspeed_MS4525::get_temperature(float &temperature)
if ((AP_HAL::millis() - _last_sample_time_ms) > 100) {
return false;
}
if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
if (_temp_count > 0) {
_temperature = _temp_sum / _temp_count;
_temp_count = 0;
_temp_sum = 0;
}
sem->give();
WITH_SEMAPHORE(sem);
if (_temp_count > 0) {
_temperature = _temp_sum / _temp_count;
_temp_count = 0;
_temp_sum = 0;
}
temperature = _temperature;
return true;
}

45
libraries/AP_Airspeed/AP_Airspeed_MS5525.cpp
View File

@ -218,14 +218,13 @@ void AP_Airspeed_MS5525::calculate(void)
}
#endif
if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
pressure_sum += P_Pa;
temperature_sum += Temp_C;
press_count++;
temp_count++;
last_sample_time_ms = AP_HAL::millis();
sem->give();
}
WITH_SEMAPHORE(sem);
pressure_sum += P_Pa;
temperature_sum += Temp_C;
press_count++;
temp_count++;
last_sample_time_ms = AP_HAL::millis();
}
// 80Hz timer
@ -287,15 +286,16 @@ bool AP_Airspeed_MS5525::get_differential_pressure(float &_pressure)
if ((AP_HAL::millis() - last_sample_time_ms) > 100) {
return false;
}
if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
if (press_count > 0) {
pressure = pressure_sum / press_count;
press_count = 0;
pressure_sum = 0;
}
sem->give();
WITH_SEMAPHORE(sem);
if (press_count > 0) {
pressure = pressure_sum / press_count;
press_count = 0;
pressure_sum = 0;
}
_pressure = pressure;
return true;
}
@ -305,14 +305,15 @@ bool AP_Airspeed_MS5525::get_temperature(float &_temperature)
if ((AP_HAL::millis() - last_sample_time_ms) > 100) {
return false;
}
if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
if (temp_count > 0) {
temperature = temperature_sum / temp_count;
temp_count = 0;
temperature_sum = 0;
}
sem->give();
WITH_SEMAPHORE(sem);
if (temp_count > 0) {
temperature = temperature_sum / temp_count;
temp_count = 0;
temperature_sum = 0;
}
_temperature = temperature;
return true;
}

36
libraries/AP_Airspeed/AP_Airspeed_SDP3X.cpp
View File

@ -180,14 +180,13 @@ void AP_Airspeed_SDP3X::_timer()
float diff_press_pa = float(P) / float(_scale);
float temperature = float(temp) / SDP3X_SCALE_TEMPERATURE;
if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
_press_sum += diff_press_pa;
_temp_sum += temperature;
_press_count++;
_temp_count++;
_last_sample_time_ms = now;
sem->give();
}
WITH_SEMAPHORE(sem);
_press_sum += diff_press_pa;
_temp_sum += temperature;
_press_count++;
_temp_count++;
_last_sample_time_ms = now;
}
/*
@ -279,14 +278,16 @@ bool AP_Airspeed_SDP3X::get_differential_pressure(float &pressure)
if (now - _last_sample_time_ms > 100) {
return false;
}
if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
{
WITH_SEMAPHORE(sem);
if (_press_count > 0) {
_press = _press_sum / _press_count;
_press_count = 0;
_press_sum = 0;
}
sem->give();
}
pressure = _correct_pressure(_press);
return true;
}
@ -297,14 +298,15 @@ bool AP_Airspeed_SDP3X::get_temperature(float &temperature)
if ((AP_HAL::millis() - _last_sample_time_ms) > 100) {
return false;
}
if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
if (_temp_count > 0) {
_temp = _temp_sum / _temp_count;
_temp_count = 0;
_temp_sum = 0;
}
sem->give();
WITH_SEMAPHORE(sem);
if (_temp_count > 0) {
_temp = _temp_sum / _temp_count;
_temp_count = 0;
_temp_sum = 0;
}
temperature = _temp;
return true;
}

10
libraries/AP_Airspeed/AP_Airspeed_UAVCAN.cpp
View File

@ -17,7 +17,7 @@ extern const AP_HAL::HAL& hal;
UC_REGISTRY_BINDER(AirspeedCb, uavcan::equipment::air_data::RawAirData);
AP_Airspeed_UAVCAN::DetectedModules AP_Airspeed_UAVCAN::_detected_modules[] = {0};
AP_HAL::Semaphore* AP_Airspeed_UAVCAN::_sem_registry = nullptr;
HAL_Semaphore AP_Airspeed_UAVCAN::_sem_registry;
// constructor
AP_Airspeed_UAVCAN::AP_Airspeed_UAVCAN(AP_Airspeed &_frontend, uint8_t _instance) :
@ -43,16 +43,12 @@ void AP_Airspeed_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan)
bool AP_Airspeed_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_Airspeed_UAVCAN::give_registry()
{
_sem_registry->give();
_sem_registry.give();
}
AP_Airspeed_Backend* AP_Airspeed_UAVCAN::probe(AP_Airspeed &_frontend, uint8_t _instance)

2
libraries/AP_Airspeed/AP_Airspeed_UAVCAN.h
View File

@ -44,5 +44,5 @@ private:
AP_Airspeed_UAVCAN *driver;
} _detected_modules[AIRSPEED_MAX_SENSORS];
static AP_HAL::Semaphore *_sem_registry;
static HAL_Semaphore _sem_registry;
};