Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 20:48:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: 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 e4e793b295
commit a260792e9e
8 changed files with 60 additions and 75 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
libraries/AP_Compass/AP_Compass_AK8963.cpp
View File

@ -17,6 +17,7 @@
#include <AP_Math/AP_Math.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_Common/Semaphore.h>
#include "AP_Compass_AK8963.h"
#include <AP_InertialSensor/AP_InertialSensor_Invensense.h>
@ -179,19 +180,19 @@ void AP_Compass_AK8963::read()
return;
}
if (_sem->take_nonblocking()) {
if (_accum_count == 0) {
/* We're not ready to publish */
_sem->give();
return;
}
if (_accum_count == 0) {
return;
}
Vector3f field = Vector3f(_mag_x_accum, _mag_y_accum, _mag_z_accum) / _accum_count;
Vector3f field;
{
WITH_SEMAPHORE(_sem);
field = Vector3f(_mag_x_accum, _mag_y_accum, _mag_z_accum) / _accum_count;
_mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
_accum_count = 0;
_sem->give();
publish_filtered_field(field, _compass_instance);
}
publish_filtered_field(field, _compass_instance);
}
void AP_Compass_AK8963::_make_adc_sensitivity_adjustment(Vector3f& field) const
@ -242,18 +243,17 @@ void AP_Compass_AK8963::_update()
// correct raw_field for known errors
correct_field(raw_field, _compass_instance);
if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
_mag_x_accum += raw_field.x;
_mag_y_accum += raw_field.y;
_mag_z_accum += raw_field.z;
_accum_count++;
if (_accum_count == 10) {
_mag_x_accum /= 2;
_mag_y_accum /= 2;
_mag_z_accum /= 2;
WITH_SEMAPHORE(_sem);
_mag_x_accum += raw_field.x;
_mag_y_accum += raw_field.y;
_mag_z_accum += raw_field.z;
_accum_count++;
if (_accum_count == 10) {
_mag_x_accum /= 2;
_mag_y_accum /= 2;
_mag_z_accum /= 2;
_accum_count = 5;
}
_sem->give();
}
}

8
libraries/AP_Compass/AP_Compass_Backend.cpp
View File

@ -11,7 +11,6 @@ extern const AP_HAL::HAL& hal;
AP_Compass_Backend::AP_Compass_Backend()
: _compass(AP::compass())
{
_sem = hal.util->new_semaphore();
}
void AP_Compass_Backend::rotate_field(Vector3f &mag, uint8_t instance)
@ -126,12 +125,9 @@ void AP_Compass_Backend::accumulate_sample(Vector3f &field, uint8_t instance,
void AP_Compass_Backend::drain_accumulated_samples(uint8_t instance,
const Vector3f *scaling)
{
if (!_sem->take_nonblocking()) {
return;
}
WITH_SEMAPHORE(_sem);
if (_accum_count == 0) {
_sem->give();
return;
}
@ -144,8 +140,6 @@ void AP_Compass_Backend::drain_accumulated_samples(uint8_t instance,
_accum.zero();
_accum_count = 0;
_sem->give();
}
/*

2
libraries/AP_Compass/AP_Compass_Backend.h
View File

@ -110,7 +110,7 @@ protected:
Compass &_compass;
// semaphore for access to shared frontend data
AP_HAL::Semaphore *_sem;
HAL_Semaphore_Recursive _sem;
// accumulated samples, protected by _sem
Vector3f _accum;

33
libraries/AP_Compass/AP_Compass_SITL.cpp
View File

@ -1,6 +1,7 @@
#include "AP_Compass_SITL.h"
#include <AP_HAL/AP_HAL.h>
#include <AP_Common/Semaphore.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
extern const AP_HAL::HAL& hal;
@ -123,9 +124,7 @@ void AP_Compass_SITL::_timer()
_mag_accum[i] += f;
}
if (!_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return;
}
WITH_SEMAPHORE(_sem);
_accum_count++;
if (_accum_count == 10) {
@ -135,28 +134,24 @@ void AP_Compass_SITL::_timer()
_accum_count = 5;
_has_sample = true;
}
_sem->give();
}
void AP_Compass_SITL::read()
{
if (_sem->take_nonblocking()) {
if (!_has_sample) {
_sem->give();
return;
}
WITH_SEMAPHORE(_sem);
for (uint8_t i=0; i<SITL_NUM_COMPASSES; i++) {
Vector3f field(_mag_accum[i]);
field /= _accum_count;
_mag_accum[i].zero();
publish_filtered_field(field, _compass_instance[i]);
}
_accum_count = 0;
_has_sample = false;
_sem->give();
if (!_has_sample) {
return;
}
for (uint8_t i=0; i<SITL_NUM_COMPASSES; i++) {
Vector3f field(_mag_accum[i]);
field /= _accum_count;
_mag_accum[i].zero();
publish_filtered_field(field, _compass_instance[i]);
}
_accum_count = 0;
_has_sample = false;
}
#endif

9
libraries/AP_Compass/AP_Compass_UAVCAN.cpp
View File

@ -36,7 +36,7 @@ UC_REGISTRY_BINDER(MagCb, uavcan::equipment::ahrs::MagneticFieldStrength);
UC_REGISTRY_BINDER(Mag2Cb, uavcan::equipment::ahrs::MagneticFieldStrength2);
AP_Compass_UAVCAN::DetectedModules AP_Compass_UAVCAN::_detected_modules[] = {0};
AP_HAL::Semaphore* AP_Compass_UAVCAN::_sem_registry = nullptr;
HAL_Semaphore AP_Compass_UAVCAN::_sem_registry;
AP_Compass_UAVCAN::AP_Compass_UAVCAN(AP_UAVCAN* ap_uavcan, uint8_t node_id, uint8_t sensor_id)
: _ap_uavcan(ap_uavcan)
@ -72,15 +72,12 @@ void AP_Compass_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan)
bool AP_Compass_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_Compass_UAVCAN::give_registry()
{
_sem_registry->give();
_sem_registry.give();
}
AP_Compass_Backend* AP_Compass_UAVCAN::probe()

2
libraries/AP_Compass/AP_Compass_UAVCAN.h
View File

@ -48,5 +48,5 @@ private:
AP_Compass_UAVCAN *driver;
} _detected_modules[COMPASS_MAX_BACKEND];
static AP_HAL::Semaphore *_sem_registry;
static HAL_Semaphore _sem_registry;
};

39
libraries/AP_Compass/Compass_learn.cpp
View File

@ -48,8 +48,6 @@ void CompassLearn::update(void)
R.from_euler(0.0f, -ToRad(inclination_deg), ToRad(declination_deg));
mag_ef = R * mag_ef;
sem = hal.util->new_semaphore();
have_earth_field = true;
// form eliptical correction matrix and invert it. This is
@ -85,8 +83,9 @@ void CompassLearn::update(void)
if (field_change.length() < min_field_change) {
return;
}
if (sem->take_nonblocking()) {
{
WITH_SEMAPHORE(sem);
// give a sample to the backend to process
new_sample.field = field;
new_sample.offsets = compass.get_offsets(0);
@ -94,7 +93,6 @@ void CompassLearn::update(void)
sample_available = true;
last_field = field;
num_samples++;
sem->give();
}
if (sample_available) {
@ -109,7 +107,9 @@ void CompassLearn::update(void)
num_samples);
}
if (!converged && sem->take_nonblocking()) {
if (!converged) {
WITH_SEMAPHORE(sem);
// stop updating the offsets once converged
compass.set_offsets(0, best_offsets);
if (num_samples > 30 && best_error < 50 && worst_error > 65) {
@ -120,7 +120,6 @@ void CompassLearn::update(void)
compass.set_learn_type(Compass::LEARN_EKF, true);
converged = true;
}
sem->give();
}
}
@ -132,13 +131,14 @@ void CompassLearn::io_timer(void)
if (!sample_available) {
return;
}
struct sample s;
if (!sem->take_nonblocking()) {
return;
{
WITH_SEMAPHORE(sem);
s = new_sample;
sample_available = false;
}
s = new_sample;
sample_available = false;
sem->give();
process_sample(s);
}
@ -192,12 +192,11 @@ void CompassLearn::process_sample(const struct sample &s)
}
}
if (sem->take_nonblocking()) {
// pass the current estimate to the front-end
best_offsets = predicted_offsets[besti];
best_error = bestv;
worst_error = worstv;
best_yaw_deg = wrap_360(degrees(s.attitude.z) + besti * (360/num_sectors));
sem->give();
}
WITH_SEMAPHORE(sem);
// pass the current estimate to the front-end
best_offsets = predicted_offsets[besti];
best_error = bestv;
worst_error = worstv;
best_yaw_deg = wrap_360(degrees(s.attitude.z) + besti * (360/num_sectors));
}

2
libraries/AP_Compass/Compass_learn.h
View File

@ -30,7 +30,7 @@ private:
Vector3f mag_ef;
// semaphore for access to shared data with IO thread
AP_HAL::Semaphore *sem;
HAL_Semaphore sem;
struct sample {
// milliGauss body field and offsets