zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Baro: convert to use WITH_SEMAPHORE()

mission-4.1.18
Andrew Tridgell 7 years ago
parent
3ef910ff11
commit
b2cc992e0c
13 changed files with 109 additions and 124 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 15
      libraries/AP_Baro/AP_Baro_BMP085.cpp
  2. 23
      libraries/AP_Baro/AP_Baro_BMP280.cpp
  3. 5
      libraries/AP_Baro/AP_Baro_Backend.cpp
  4. 3
      libraries/AP_Baro/AP_Baro_Backend.h
  5. 10
      libraries/AP_Baro/AP_Baro_DPS280.cpp
  6. 10
      libraries/AP_Baro/AP_Baro_FBM320.cpp
  7. 41
      libraries/AP_Baro/AP_Baro_ICM20789.cpp
  8. 35
      libraries/AP_Baro/AP_Baro_KellerLD.cpp
  9. 15
      libraries/AP_Baro/AP_Baro_LPS2XH.cpp
  10. 47
      libraries/AP_Baro/AP_Baro_MS5611.cpp
  11. 6
      libraries/AP_Baro/AP_Baro_SITL.cpp
  12. 21
      libraries/AP_Baro/AP_Baro_UAVCAN.cpp
  13. 2
      libraries/AP_Baro/AP_Baro_UAVCAN.h

15
libraries/AP_Baro/AP_Baro_BMP085.cpp
Unescape View File

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

23
libraries/AP_Baro/AP_Baro_BMP280.cpp
Unescape View File

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

5
libraries/AP_Baro/AP_Baro_Backend.cpp
Unescape View File

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

3
libraries/AP_Baro/AP_Baro_Backend.h
Unescape View File

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

10
libraries/AP_Baro/AP_Baro_DPS280.cpp
Unescape View File

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

10
libraries/AP_Baro/AP_Baro_FBM320.cpp
Unescape View File

@ -187,12 +187,12 @@ void AP_Baro_FBM320::timer(void)
} else { } else {
int32_t pressure, temperature; int32_t pressure, temperature;
calculate_PT(value_T, value, 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; pressure_sum += pressure;
// sum and convert to degrees // sum and convert to degrees
temperature_sum += temperature*0.01; temperature_sum += temperature*0.01;
count++; count++;
_sem->give(); _sem.give();
} }
} }
@ -207,9 +207,9 @@ void AP_Baro_FBM320::timer(void)
// transfer data to the frontend // transfer data to the frontend
void AP_Baro_FBM320::update(void) void AP_Baro_FBM320::update(void)
{ {
if (count != 0 && _sem->take_nonblocking()) { if (count != 0 && _sem.take_nonblocking()) {
if (count == 0) { if (count == 0) {
_sem->give(); _sem.give();
return; return;
} }
@ -218,6 +218,6 @@ void AP_Baro_FBM320::update(void)
temperature_sum = 0; temperature_sum = 0;
count=0; count=0;
_sem->give(); _sem.give();
} }
} }

41
libraries/AP_Baro/AP_Baro_ICM20789.cpp
Unescape View File

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

35
libraries/AP_Baro/AP_Baro_KellerLD.cpp
Unescape View File

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

15
libraries/AP_Baro/AP_Baro_LPS2XH.cpp
Unescape View File

@ -205,15 +205,15 @@ void AP_Baro_LPS2XH::_timer(void)
// transfer data to the frontend // transfer data to the frontend
void AP_Baro_LPS2XH::update(void) void AP_Baro_LPS2XH::update(void)
{ {
if (_sem->take_nonblocking()) { if (_sem.take_nonblocking()) {
if (!_has_sample) { if (!_has_sample) {
_sem->give(); _sem.give();
return; return;
} }
_copy_to_frontend(_instance, _pressure, _temperature); _copy_to_frontend(_instance, _pressure, _temperature);
_has_sample = false; _has_sample = false;
_sem->give(); _sem.give();
} }
} }
@ -223,14 +223,15 @@ void AP_Baro_LPS2XH::_update_temperature(void)
uint8_t pu8[2]; uint8_t pu8[2];
_dev->read_registers(TEMP_OUT_ADDR, pu8, 2); _dev->read_registers(TEMP_OUT_ADDR, pu8, 2);
int16_t Temp_Reg_s16 = (uint16_t)(pu8[1]<<8) | pu8[0]; 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){ if(_lps2xh_type == BARO_LPS25H){
_temperature=((float)(Temp_Reg_s16/480)+42.5); _temperature=((float)(Temp_Reg_s16/480)+42.5);
} }
if(_lps2xh_type == BARO_LPS22H){ if(_lps2xh_type == BARO_LPS22H){
_temperature=(float)(Temp_Reg_s16/100); _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); _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_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 int32_t Pressure_mb = Pressure_Reg_s32 * (100.0 / 4096); // scale for pa
if (_sem->take_nonblocking()) { if (_sem.take_nonblocking()) {
_pressure=Pressure_mb; _pressure=Pressure_mb;
_sem->give(); _sem.give();
} }
} }

47
libraries/AP_Baro/AP_Baro_MS5611.cpp
Unescape View File

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

6
libraries/AP_Baro/AP_Baro_SITL.cpp
Unescape View File

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

21
libraries/AP_Baro/AP_Baro_UAVCAN.cpp
Unescape View File

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

2
libraries/AP_Baro/AP_Baro_UAVCAN.h
Unescape View File

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

Write Preview
Loading…
Cancel
Save