Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_IRLock: 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 755dc8dc5d
commit dedfa54d5a
2 changed files with 12 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AP_IRLock/AP_IRLock_I2C.cpp
View File

@ -43,8 +43,6 @@ void AP_IRLock_I2C::init(int8_t bus)
return; return;
} }
sem = hal.util->new_semaphore();
// read at 50Hz // read at 50Hz
printf("Starting IRLock on I2C\n"); printf("Starting IRLock on I2C\n");
@ -132,14 +130,15 @@ void AP_IRLock_I2C::read_frames(void)
pixel_to_1M_plane(corner1_pix_x, corner1_pix_y, corner1_pos_x, corner1_pos_y); pixel_to_1M_plane(corner1_pix_x, corner1_pix_y, corner1_pos_x, corner1_pos_y);
pixel_to_1M_plane(corner2_pix_x, corner2_pix_y, corner2_pos_x, corner2_pos_y); pixel_to_1M_plane(corner2_pix_x, corner2_pix_y, corner2_pos_x, corner2_pos_y);
if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { {
WITH_SEMAPHORE(sem);
/* convert to angles */ /* convert to angles */
_target_info.timestamp = AP_HAL::millis(); _target_info.timestamp = AP_HAL::millis();
_target_info.pos_x = 0.5f*(corner1_pos_x+corner2_pos_x); _target_info.pos_x = 0.5f*(corner1_pos_x+corner2_pos_x);
_target_info.pos_y = 0.5f*(corner1_pos_y+corner2_pos_y); _target_info.pos_y = 0.5f*(corner1_pos_y+corner2_pos_y);
_target_info.size_x = corner2_pos_x-corner1_pos_x; _target_info.size_x = corner2_pos_x-corner1_pos_x;
_target_info.size_y = corner2_pos_y-corner1_pos_y; _target_info.size_y = corner2_pos_y-corner1_pos_y;
sem->give();
} }
#if 0 #if 0
@ -158,17 +157,17 @@ void AP_IRLock_I2C::read_frames(void)
bool AP_IRLock_I2C::update() bool AP_IRLock_I2C::update()
{ {
bool new_data = false; bool new_data = false;
if (!dev || !sem) { if (!dev) {
return false; return false;
} }
if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { WITH_SEMAPHORE(sem);
if (_last_update_ms != _target_info.timestamp) {
new_data = true; if (_last_update_ms != _target_info.timestamp) {
} new_data = true;
_last_update_ms = _target_info.timestamp;
_flags.healthy = (AP_HAL::millis() - _last_read_ms < 100);
sem->give();
} }
_last_update_ms = _target_info.timestamp;
_flags.healthy = (AP_HAL::millis() - _last_read_ms < 100);
// return true if new data found // return true if new data found
return new_data; return new_data;
} }

2
libraries/AP_IRLock/AP_IRLock_I2C.h
View File

@ -35,6 +35,6 @@ private:
void pixel_to_1M_plane(float pix_x, float pix_y, float &ret_x, float &ret_y); void pixel_to_1M_plane(float pix_x, float pix_y, float &ret_x, float &ret_y);
AP_HAL::Semaphore *sem; HAL_Semaphore sem;
uint32_t _last_read_ms; uint32_t _last_read_ms;
}; };