Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 07:13:56 -04:00
Code Issues 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), frontend(_frontend),
instance(_instance) instance(_instance)
{ {
sem = hal.util->new_semaphore();
} }
AP_Airspeed_Backend::~AP_Airspeed_Backend(void) 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 // semaphore for access to shared frontend data
AP_HAL::Semaphore *sem; HAL_Semaphore sem;
float get_airspeed_ratio(void) const { float get_airspeed_ratio(void) const {
return frontend.get_airspeed_ratio(instance); 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 press_h2o = 1.25f * 2.0f * DLVR_FSS * ((pres_raw - DLVR_OFFSET) / DLVR_SCALE);
float temp = temp_raw * (200.0f / 2047.0f) - 50.0f; 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; pressure_sum += INCH_OF_H2O_TO_PASCAL * press_h2o;
temperature_sum += temp; temperature_sum += temp;
press_count++; press_count++;
temp_count++; temp_count++;
last_sample_time_ms = AP_HAL::millis(); last_sample_time_ms = AP_HAL::millis();
sem->give();
} }
// return the current differential_pressure in Pascal // 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) { if ((AP_HAL::millis() - last_sample_time_ms) > 100) {
return false; return false;
} }
if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
{
WITH_SEMAPHORE(sem);
if (press_count > 0) { if (press_count > 0) {
pressure = pressure_sum / press_count; pressure = pressure_sum / press_count;
press_count = 0; press_count = 0;
pressure_sum = 0; pressure_sum = 0;
} }
sem->give();
} }
_pressure = pressure; _pressure = pressure;
return true; return true;
} }
@ -115,14 +117,15 @@ bool AP_Airspeed_DLVR::get_temperature(float &_temperature)
if ((AP_HAL::millis() - last_sample_time_ms) > 100) { if ((AP_HAL::millis() - last_sample_time_ms) > 100) {
return false; return false;
} }
if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
if (temp_count > 0) { WITH_SEMAPHORE(sem);
temperature = temperature_sum / temp_count;
temp_count = 0; if (temp_count > 0) {
temperature_sum = 0; temperature = temperature_sum / temp_count;
} temp_count = 0;
sem->give(); temperature_sum = 0;
} }
_temperature = temperature; _temperature = temperature;
return true; return true;
} }

51
libraries/AP_Airspeed/AP_Airspeed_MS4525.cpp
View File

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

45
libraries/AP_Airspeed/AP_Airspeed_MS5525.cpp
View File

@ -218,14 +218,13 @@ void AP_Airspeed_MS5525::calculate(void)
} }
#endif #endif
if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { WITH_SEMAPHORE(sem);
pressure_sum += P_Pa;
temperature_sum += Temp_C; pressure_sum += P_Pa;
press_count++; temperature_sum += Temp_C;
temp_count++; press_count++;
last_sample_time_ms = AP_HAL::millis(); temp_count++;
sem->give(); last_sample_time_ms = AP_HAL::millis();
}
} }
// 80Hz timer // 80Hz timer
@ -287,15 +286,16 @@ bool AP_Airspeed_MS5525::get_differential_pressure(float &_pressure)
if ((AP_HAL::millis() - last_sample_time_ms) > 100) { if ((AP_HAL::millis() - last_sample_time_ms) > 100) {
return false; return false;
} }
if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
if (press_count > 0) { WITH_SEMAPHORE(sem);
pressure = pressure_sum / press_count;
press_count = 0; if (press_count > 0) {
pressure_sum = 0; pressure = pressure_sum / press_count;
} press_count = 0;
sem->give(); pressure_sum = 0;
} }
_pressure = pressure; _pressure = pressure;
return true; return true;
} }
@ -305,14 +305,15 @@ bool AP_Airspeed_MS5525::get_temperature(float &_temperature)
if ((AP_HAL::millis() - last_sample_time_ms) > 100) { if ((AP_HAL::millis() - last_sample_time_ms) > 100) {
return false; return false;
} }
if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
if (temp_count > 0) { WITH_SEMAPHORE(sem);
temperature = temperature_sum / temp_count;
temp_count = 0; if (temp_count > 0) {
temperature_sum = 0; temperature = temperature_sum / temp_count;
} temp_count = 0;
sem->give(); temperature_sum = 0;
} }
_temperature = temperature; _temperature = temperature;
return true; 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 diff_press_pa = float(P) / float(_scale);
float temperature = float(temp) / SDP3X_SCALE_TEMPERATURE; float temperature = float(temp) / SDP3X_SCALE_TEMPERATURE;
if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { WITH_SEMAPHORE(sem);
_press_sum += diff_press_pa;
_temp_sum += temperature; _press_sum += diff_press_pa;
_press_count++; _temp_sum += temperature;
_temp_count++; _press_count++;
_last_sample_time_ms = now; _temp_count++;
sem->give(); _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) { if (now - _last_sample_time_ms > 100) {
return false; return false;
} }
if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
{
WITH_SEMAPHORE(sem);
if (_press_count > 0) { if (_press_count > 0) {
_press = _press_sum / _press_count; _press = _press_sum / _press_count;
_press_count = 0; _press_count = 0;
_press_sum = 0; _press_sum = 0;
} }
sem->give();
} }
pressure = _correct_pressure(_press); pressure = _correct_pressure(_press);
return true; return true;
} }
@ -297,14 +298,15 @@ bool AP_Airspeed_SDP3X::get_temperature(float &temperature)
if ((AP_HAL::millis() - _last_sample_time_ms) > 100) { if ((AP_HAL::millis() - _last_sample_time_ms) > 100) {
return false; return false;
} }
if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
if (_temp_count > 0) { WITH_SEMAPHORE(sem);
_temp = _temp_sum / _temp_count;
_temp_count = 0; if (_temp_count > 0) {
_temp_sum = 0; _temp = _temp_sum / _temp_count;
} _temp_count = 0;
sem->give(); _temp_sum = 0;
} }
temperature = _temp; temperature = _temp;
return true; 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); UC_REGISTRY_BINDER(AirspeedCb, uavcan::equipment::air_data::RawAirData);
AP_Airspeed_UAVCAN::DetectedModules AP_Airspeed_UAVCAN::_detected_modules[] = {0}; 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 // constructor
AP_Airspeed_UAVCAN::AP_Airspeed_UAVCAN(AP_Airspeed &_frontend, uint8_t _instance) : 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() bool AP_Airspeed_UAVCAN::take_registry()
{ {
if (_sem_registry == nullptr) { return _sem_registry.take(HAL_SEMAPHORE_BLOCK_FOREVER);
_sem_registry = hal.util->new_semaphore();
}
return _sem_registry->take(HAL_SEMAPHORE_BLOCK_FOREVER);
} }
void AP_Airspeed_UAVCAN::give_registry() 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) 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; AP_Airspeed_UAVCAN *driver;
} _detected_modules[AIRSPEED_MAX_SENSORS]; } _detected_modules[AIRSPEED_MAX_SENSORS];
static AP_HAL::Semaphore *_sem_registry; static HAL_Semaphore _sem_registry;
}; };