Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 04:03:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

Optflow: uses new Semaphore

Browse Source
This commit is contained in:
Pat Hickey 2013-01-03 13:32:56 -08:00
parent d808c19c10
commit 8503f3e2ae
2 changed files with 16 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp
View File

@ -82,15 +82,20 @@ AP_OpticalFlow_ADNS3080::init()
_spi = NULL; _spi = NULL;
finish: finish:
// resume timer
hal.scheduler->resume_timer_procs();
// if device is working register the global static read function to // if device is working register the global static read function to
// be called at 1khz // be called at 1khz
if( retvalue ) { if( retvalue ) {
hal.scheduler->register_timer_process( AP_OpticalFlow_ADNS3080::read ); hal.scheduler->register_timer_process( AP_OpticalFlow_ADNS3080::read );
_spi_sem = _spi->get_semaphore();
if (_spi_sem == NULL) {
hal.scheduler->panic(PSTR("PANIC: Got SPI Driver, but did not "
"get valid SPI semaphore in AP_Optflow_ADNS3080"));
}
} }
// resume timer
hal.scheduler->resume_timer_procs();
return retvalue; return retvalue;
} }
@ -100,13 +105,8 @@ uint8_t AP_OpticalFlow_ADNS3080::read_register(uint8_t address)
{ {
if (_spi == NULL) return 0; if (_spi == NULL) return 0;
// get spi semaphore if required if (!_spi_sem->take_nonblocking()) {
AP_HAL::Semaphore* sem = _spi->get_semaphore(); return 0;
if( sem != NULL ) {
// if failed to get semaphore then just quietly fail
if( !sem->get(this) ) {
return 0;
}
} }
_spi->cs_assert(); _spi->cs_assert();
@ -118,10 +118,7 @@ uint8_t AP_OpticalFlow_ADNS3080::read_register(uint8_t address)
_spi->cs_release(); _spi->cs_release();
// get spi semaphore if required _spi_sem->give();
if( sem != NULL ) {
sem->release(this);
}
return result; return result;
} }
@ -131,13 +128,8 @@ void AP_OpticalFlow_ADNS3080::write_register(uint8_t address, uint8_t value)
{ {
if (_spi == NULL) return; if (_spi == NULL) return;
AP_HAL::Semaphore* sem = _spi->get_semaphore(); if (!_spi_sem->take_nonblocking()) {
// get spi semaphore if required return;
if( sem != NULL ) {
// if failed to get semaphore then just quietly fail
if( !sem->get(this) ) {
return;
}
} }
_spi->cs_assert(); _spi->cs_assert();
@ -149,11 +141,8 @@ void AP_OpticalFlow_ADNS3080::write_register(uint8_t address, uint8_t value)
_spi->transfer(value); _spi->transfer(value);
_spi->cs_release(); _spi->cs_release();
// get spi3 semaphore if required _spi_sem->give();
if( sem != NULL ) {
sem->release(this);
}
} }
// reset sensor by holding a pin high (or is it low?) for 10us. // reset sensor by holding a pin high (or is it low?) for 10us.

1
libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h
View File

@ -158,6 +158,7 @@ private:
// SPI device // SPI device
AP_HAL::SPIDeviceDriver *_spi; AP_HAL::SPIDeviceDriver *_spi;
AP_HAL::Semaphore *_spi_sem;
}; };
#endif #endif