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

AP_Baro: convert to use WITH_SEMAPHORE()

Browse Source
This commit is contained in:
Andrew Tridgell 2018-08-16 12:53:17 +10:00
parent 3ef910ff11
commit b2cc992e0c
13 changed files with 111 additions and 126 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_Baro/AP_Baro_BMP085.cpp
View File

@ -203,9 +203,9 @@ void AP_Baro_BMP085::_timer(void)
*/
void AP_Baro_BMP085::update(void)
{
if (_sem->take_nonblocking()) {
if (_sem.take_nonblocking()) {
if (!_has_sample) {
_sem->give();
_sem.give();
return;
}
@ -213,7 +213,7 @@ void AP_Baro_BMP085::update(void)
float pressure = _pressure_filter.getf();
_copy_to_frontend(_instance, pressure, temperature);
_sem->give();
_sem.give();
}
}
@ -312,11 +312,10 @@ void AP_Baro_BMP085::_calculate()
return;
}
if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
_pressure_filter.apply(p);
_has_sample = true;
_sem->give();
}
WITH_SEMAPHORE(_sem);
_pressure_filter.apply(p);
_has_sample = true;
}
bool AP_Baro_BMP085::_data_ready()

23
libraries/AP_Baro/AP_Baro_BMP280.cpp
View File

@ -140,15 +140,15 @@ void AP_Baro_BMP280::_timer(void)
// transfer data to the frontend
void AP_Baro_BMP280::update(void)
{
if (_sem->take_nonblocking()) {
if (_sem.take_nonblocking()) {
if (!_has_sample) {
_sem->give();
_sem.give();
return;
}
_copy_to_frontend(_instance, _pressure, _temperature);
_has_sample = false;
_sem->give();
_sem.give();
}
}
@ -165,10 +165,9 @@ void AP_Baro_BMP280::_update_temperature(int32_t temp_raw)
const float temp = ((float)t) / 100.0f;
if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
_temperature = temp;
_sem->give();
}
WITH_SEMAPHORE(_sem);
_temperature = temp;
}
// calculate pressure
@ -199,9 +198,9 @@ void AP_Baro_BMP280::_update_pressure(int32_t press_raw)
if (!pressure_ok(press)) {
return;
}
if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
_pressure = press;
_has_sample = true;
_sem->give();
}
WITH_SEMAPHORE(_sem);
_pressure = press;
_has_sample = true;
}

5
libraries/AP_Baro/AP_Baro_Backend.cpp
View File

@ -7,7 +7,6 @@ extern const AP_HAL::HAL& hal;
AP_Baro_Backend::AP_Baro_Backend(AP_Baro &baro) :
_frontend(baro)
{
_sem = hal.util->new_semaphore();
}
void AP_Baro_Backend::update_healthy_flag(uint8_t instance)
@ -15,7 +14,7 @@ void AP_Baro_Backend::update_healthy_flag(uint8_t instance)
if (instance >= _frontend._num_sensors) {
return;
}
if (!_sem->take_nonblocking()) {
if (!_sem.take_nonblocking()) {
return;
}
@ -27,7 +26,7 @@ void AP_Baro_Backend::update_healthy_flag(uint8_t instance)
(now - _frontend.sensors[instance].last_change_ms < BARO_DATA_CHANGE_TIMEOUT_MS) &&
!is_zero(_frontend.sensors[instance].pressure);
_sem->give();
_sem.give();
}
void AP_Baro_Backend::backend_update(uint8_t instance)

3
libraries/AP_Baro/AP_Baro_Backend.h
View File

@ -1,6 +1,7 @@
#pragma once
#include "AP_Baro.h"
#include <AP_Common/Semaphore.h>
class AP_Baro_Backend
{
@ -33,7 +34,7 @@ protected:
void _copy_to_frontend(uint8_t instance, float pressure, float temperature);
// semaphore for access to shared frontend data
AP_HAL::Semaphore *_sem;
HAL_Semaphore _sem;
virtual void update_healthy_flag(uint8_t instance);

10
libraries/AP_Baro/AP_Baro_DPS280.cpp
View File

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

10
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) && _sem.take_nonblocking()) {
pressure_sum += pressure;
// sum and convert to degrees
temperature_sum += temperature*0.01;
count++;
_sem->give();
_sem.give();
}
}
@ -207,9 +207,9 @@ 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.take_nonblocking()) {
if (count == 0) {
_sem->give();
_sem.give();
return;
}
@ -218,6 +218,6 @@ void AP_Baro_FBM320::update(void)
temperature_sum = 0;
count=0;
_sem->give();
_sem.give();
}
}

41
libraries/AP_Baro/AP_Baro_ICM20789.cpp
View File

@ -305,19 +305,18 @@ void AP_Baro_ICM20789::convert_data(uint32_t Praw, uint32_t Traw)
return;
}
if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
WITH_SEMAPHORE(_sem);
#if BARO_ICM20789_DEBUG
dd.Praw = Praw;
dd.Traw = Traw;
dd.P = P;
dd.T = T;
dd.Praw = Praw;
dd.Traw = Traw;
dd.P = P;
dd.T = T;
#endif
accum.psum += P;
accum.tsum += T;
accum.count++;
_sem->give();
}
accum.psum += P;
accum.tsum += T;
accum.count++;
}
void AP_Baro_ICM20789::timer(void)
@ -342,19 +341,19 @@ void AP_Baro_ICM20789::timer(void)
void AP_Baro_ICM20789::update()
{
if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
if (accum.count > 0) {
_copy_to_frontend(instance, accum.psum/accum.count, accum.tsum/accum.count);
accum.psum = accum.tsum = 0;
accum.count = 0;
}
_sem->give();
#if BARO_ICM20789_DEBUG
// useful for debugging
DataFlash_Class::instance()->Log_Write("ICMB", "TimeUS,Traw,Praw,P,T", "QIIff",
AP_HAL::micros64(),
dd.Traw, dd.Praw, dd.P, dd.T);
// useful for debugging
DataFlash_Class::instance()->Log_Write("ICMB", "TimeUS,Traw,Praw,P,T", "QIIff",
AP_HAL::micros64(),
dd.Traw, dd.Praw, dd.P, dd.T);
#endif
WITH_SEMAPHORE(_sem);
if (accum.count > 0) {
_copy_to_frontend(instance, accum.psum/accum.count, accum.tsum/accum.count);
accum.psum = accum.tsum = 0;
accum.count = 0;
}
}

37
libraries/AP_Baro/AP_Baro_KellerLD.cpp
View File

@ -191,12 +191,12 @@ bool AP_Baro_KellerLD::_read()
if (!pressure_ok(pressure_raw)) {
return false;
}
if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
_update_and_wrap_accumulator(pressure_raw, temperature_raw, 128);
_sem->give();
return true;
}
return false;
WITH_SEMAPHORE(_sem);
_update_and_wrap_accumulator(pressure_raw, temperature_raw, 128);
return true;
}
// Periodic callback, regular update at 50Hz
@ -230,22 +230,19 @@ void AP_Baro_KellerLD::update()
float sum_pressure, sum_temperature;
float num_samples;
if (!_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return;
{
WITH_SEMAPHORE(_sem);
if (_accum.num_samples == 0) {
return;
}
sum_pressure = _accum.sum_pressure;
sum_temperature = _accum.sum_temperature;
num_samples = _accum.num_samples;
memset(&_accum, 0, sizeof(_accum));
}
if (_accum.num_samples == 0) {
_sem->give();
return;
}
sum_pressure = _accum.sum_pressure;
sum_temperature = _accum.sum_temperature;
num_samples = _accum.num_samples;
memset(&_accum, 0, sizeof(_accum));
_sem->give();
uint16_t raw_pressure_avg = sum_pressure / num_samples;
uint16_t raw_temperature_avg = sum_temperature / num_samples;

15
libraries/AP_Baro/AP_Baro_LPS2XH.cpp
View File

@ -205,15 +205,15 @@ void AP_Baro_LPS2XH::_timer(void)
// transfer data to the frontend
void AP_Baro_LPS2XH::update(void)
{
if (_sem->take_nonblocking()) {
if (_sem.take_nonblocking()) {
if (!_has_sample) {
_sem->give();
_sem.give();
return;
}
_copy_to_frontend(_instance, _pressure, _temperature);
_has_sample = false;
_sem->give();
_sem.give();
}
}
@ -223,14 +223,15 @@ void AP_Baro_LPS2XH::_update_temperature(void)
uint8_t pu8[2];
_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 (_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();
_sem.give();
}
}
@ -241,8 +242,8 @@ 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()) {
if (_sem.take_nonblocking()) {
_pressure=Pressure_mb;
_sem->give();
_sem.give();
}
}

49
libraries/AP_Baro/AP_Baro_MS5611.cpp
View File

@ -304,19 +304,17 @@ void AP_Baro_MS56XX::_timer(void)
return;
}
if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
if (_state == 0) {
_update_and_wrap_accumulator(&_accum.s_D2, adc_val,
&_accum.d2_count, 32);
} else {
if (pressure_ok(adc_val)) {
_update_and_wrap_accumulator(&_accum.s_D1, adc_val,
&_accum.d1_count, 128);
}
}
_sem->give();
_state = next_state;
WITH_SEMAPHORE(_sem);
if (_state == 0) {
_update_and_wrap_accumulator(&_accum.s_D2, adc_val,
&_accum.d2_count, 32);
} else if (pressure_ok(adc_val)) {
_update_and_wrap_accumulator(&_accum.s_D1, adc_val,
&_accum.d1_count, 128);
}
_state = next_state;
}
void AP_Baro_MS56XX::_update_and_wrap_accumulator(uint32_t *accum, uint32_t val,
@ -335,23 +333,20 @@ void AP_Baro_MS56XX::update()
uint32_t sD1, sD2;
uint8_t d1count, d2count;
if (!_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return;
{
WITH_SEMAPHORE(_sem);
if (_accum.d1_count == 0) {
return;
}
sD1 = _accum.s_D1;
sD2 = _accum.s_D2;
d1count = _accum.d1_count;
d2count = _accum.d2_count;
memset(&_accum, 0, sizeof(_accum));
}
if (_accum.d1_count == 0) {
_sem->give();
return;
}
sD1 = _accum.s_D1;
sD2 = _accum.s_D2;
d1count = _accum.d1_count;
d2count = _accum.d2_count;
memset(&_accum, 0, sizeof(_accum));
_sem->give();
if (d1count != 0) {
_D1 = ((float)sD1) / d1count;
}

6
libraries/AP_Baro/AP_Baro_SITL.cpp
View File

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

21
libraries/AP_Baro/AP_Baro_UAVCAN.cpp
View File

@ -16,7 +16,6 @@ extern const AP_HAL::HAL& hal;
AP_Baro_UAVCAN::AP_Baro_UAVCAN(AP_Baro &baro) :
AP_Baro_Backend(baro)
{
_sem_baro = hal.util->new_semaphore();
}
AP_Baro_UAVCAN::~AP_Baro_UAVCAN()
@ -31,7 +30,6 @@ AP_Baro_UAVCAN::~AP_Baro_UAVCAN()
}
ap_uavcan->remove_baro_listener(this);
delete _sem_baro;
debug_baro_uavcan(2, _manager, "AP_Baro_UAVCAN destructed\n\r");
}
@ -66,22 +64,19 @@ AP_Baro_Backend *AP_Baro_UAVCAN::probe(AP_Baro &baro)
// Read the sensor
void AP_Baro_UAVCAN::update(void)
{
if (_sem_baro->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
_copy_to_frontend(_instance, _pressure, _temperature);
WITH_SEMAPHORE(_sem_baro);
_copy_to_frontend(_instance, _pressure, _temperature);
_frontend.set_external_temperature(_temperature);
_sem_baro->give();
}
_frontend.set_external_temperature(_temperature);
}
void AP_Baro_UAVCAN::handle_baro_msg(float pressure, float temperature)
{
if (_sem_baro->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
_pressure = pressure;
_temperature = temperature - C_TO_KELVIN;
_last_timestamp = AP_HAL::micros64();
_sem_baro->give();
}
WITH_SEMAPHORE(_sem_baro);
_pressure = pressure;
_temperature = temperature - C_TO_KELVIN;
_last_timestamp = AP_HAL::micros64();
}
bool AP_Baro_UAVCAN::register_uavcan_baro(uint8_t mgr, uint8_t node)

2
libraries/AP_Baro/AP_Baro_UAVCAN.h
View File

@ -26,5 +26,5 @@ private:
bool _initialized;
AP_HAL::Semaphore *_sem_baro;
HAL_Semaphore _sem_baro;
};