AP_OpticalFlow: use WITH_SEMAPHORE()

and removed usage of hal.util->new_semaphore()
This commit is contained in:
Andrew Tridgell 2018-10-12 10:35:04 +11:00
parent bf829cd792
commit a1f1e7e435
4 changed files with 13 additions and 25 deletions

View File

@ -71,12 +71,10 @@ bool AP_OpticalFlow_PX4Flow::scan_buses(void)
if (!tdev) { if (!tdev) {
continue; continue;
} }
if (!tdev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { WITH_SEMAPHORE(tdev->get_semaphore());
continue;
}
struct i2c_integral_frame frame; struct i2c_integral_frame frame;
success = tdev->read_registers(REG_INTEGRAL_FRAME, (uint8_t *)&frame, sizeof(frame)); success = tdev->read_registers(REG_INTEGRAL_FRAME, (uint8_t *)&frame, sizeof(frame));
tdev->get_semaphore()->give();
if (success) { if (success) {
printf("Found PX4Flow on bus %u\n", bus); printf("Found PX4Flow on bus %u\n", bus);
dev = std::move(tdev); dev = std::move(tdev);

View File

@ -107,9 +107,7 @@ bool AP_OpticalFlow_Pixart::setup_sensor(void)
if (!_dev) { if (!_dev) {
return false; return false;
} }
if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { WITH_SEMAPHORE(_dev->get_semaphore());
AP_HAL::panic("Unable to get bus semaphore");
}
uint8_t id; uint8_t id;
uint16_t crc; uint16_t crc;
@ -133,7 +131,7 @@ bool AP_OpticalFlow_Pixart::setup_sensor(void)
model = PIXART_3901; model = PIXART_3901;
} else { } else {
debug("Not a recognised device\n"); debug("Not a recognised device\n");
goto failed; return false;
} }
if (model == PIXART_3900) { if (model == PIXART_3900) {
@ -142,7 +140,7 @@ bool AP_OpticalFlow_Pixart::setup_sensor(void)
id = reg_read(PIXART_REG_SROM_ID); id = reg_read(PIXART_REG_SROM_ID);
if (id != srom_id) { if (id != srom_id) {
debug("Pixart: bad SROM ID: 0x%02x\n", id); debug("Pixart: bad SROM ID: 0x%02x\n", id);
goto failed; return false;
} }
reg_write(PIXART_REG_SROM_EN, 0x15); reg_write(PIXART_REG_SROM_EN, 0x15);
@ -151,7 +149,7 @@ bool AP_OpticalFlow_Pixart::setup_sensor(void)
crc = reg_read16u(PIXART_REG_DOUT_L); crc = reg_read16u(PIXART_REG_DOUT_L);
if (crc != 0xBEEF) { if (crc != 0xBEEF) {
debug("Pixart: bad SROM CRC: 0x%04x\n", crc); debug("Pixart: bad SROM CRC: 0x%04x\n", crc);
goto failed; return false;
} }
} }
@ -167,16 +165,10 @@ bool AP_OpticalFlow_Pixart::setup_sensor(void)
debug("Pixart %s ready\n", model==PIXART_3900?"3900":"3901"); debug("Pixart %s ready\n", model==PIXART_3900?"3900":"3901");
_dev->get_semaphore()->give();
integral.last_frame_us = AP_HAL::micros(); integral.last_frame_us = AP_HAL::micros();
_dev->register_periodic_callback(2000, FUNCTOR_BIND_MEMBER(&AP_OpticalFlow_Pixart::timer, void)); _dev->register_periodic_callback(2000, FUNCTOR_BIND_MEMBER(&AP_OpticalFlow_Pixart::timer, void));
return true; return true;
failed:
_dev->get_semaphore()->give();
return false;
} }
@ -299,13 +291,14 @@ void AP_OpticalFlow_Pixart::timer(void)
float dt = dt_us * 1.0e-6; float dt = dt_us * 1.0e-6;
const Vector3f &gyro = get_ahrs().get_gyro(); const Vector3f &gyro = get_ahrs().get_gyro();
if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { {
WITH_SEMAPHORE(_sem);
integral.sum.x += burst.delta_x; integral.sum.x += burst.delta_x;
integral.sum.y += burst.delta_y; integral.sum.y += burst.delta_y;
integral.sum_us += dt_us; integral.sum_us += dt_us;
integral.last_frame_us = last_burst_us; integral.last_frame_us = last_burst_us;
integral.gyro += Vector2f(gyro.x, gyro.y) * dt; integral.gyro += Vector2f(gyro.x, gyro.y) * dt;
_sem->give();
} }
#if 0 #if 0
@ -344,7 +337,9 @@ void AP_OpticalFlow_Pixart::update(void)
state.device_id = 1; state.device_id = 1;
state.surface_quality = burst.squal; state.surface_quality = burst.squal;
if (integral.sum_us > 0 && _sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { if (integral.sum_us > 0) {
WITH_SEMAPHORE(_sem);
const Vector2f flowScaler = _flowScaler(); const Vector2f flowScaler = _flowScaler();
float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x; float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x;
float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y; float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y;
@ -362,7 +357,6 @@ void AP_OpticalFlow_Pixart::update(void)
integral.sum.zero(); integral.sum.zero();
integral.sum_us = 0; integral.sum_us = 0;
integral.gyro.zero(); integral.gyro.zero();
_sem->give();
} else { } else {
state.flowRate.zero(); state.flowRate.zero();
state.bodyRate.zero(); state.bodyRate.zero();

View File

@ -20,14 +20,10 @@ extern const AP_HAL::HAL& hal;
OpticalFlow_backend::OpticalFlow_backend(OpticalFlow &_frontend) : OpticalFlow_backend::OpticalFlow_backend(OpticalFlow &_frontend) :
frontend(_frontend) frontend(_frontend)
{ {
_sem = hal.util->new_semaphore();
} }
OpticalFlow_backend::~OpticalFlow_backend(void) OpticalFlow_backend::~OpticalFlow_backend(void)
{ {
if (_sem) {
delete _sem;
}
} }
// update the frontend // update the frontend

View File

@ -58,5 +58,5 @@ protected:
uint8_t get_address(void) const { return frontend._address; } uint8_t get_address(void) const { return frontend._address; }
// semaphore for access to shared frontend data // semaphore for access to shared frontend data
AP_HAL::Semaphore *_sem; HAL_Semaphore _sem;
}; };