Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-02 11:43:56 -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;
finish:
// resume timer
hal.scheduler->resume_timer_procs();
// if device is working register the global static read function to
// be called at 1khz
if( retvalue ) {
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;
}
@ -100,13 +105,8 @@ uint8_t AP_OpticalFlow_ADNS3080::read_register(uint8_t address)
{
if (_spi == NULL) return 0;
// get spi semaphore if required
AP_HAL::Semaphore* sem = _spi->get_semaphore();
if( sem != NULL ) {
// if failed to get semaphore then just quietly fail
if( !sem->get(this) ) {
return 0;
}
if (!_spi_sem->take_nonblocking()) {
return 0;
}
_spi->cs_assert();
@ -118,10 +118,7 @@ uint8_t AP_OpticalFlow_ADNS3080::read_register(uint8_t address)
_spi->cs_release();
// get spi semaphore if required
if( sem != NULL ) {
sem->release(this);
}
_spi_sem->give();
return result;
}
@ -131,13 +128,8 @@ void AP_OpticalFlow_ADNS3080::write_register(uint8_t address, uint8_t value)
{
if (_spi == NULL) return;
AP_HAL::Semaphore* sem = _spi->get_semaphore();
// get spi semaphore if required
if( sem != NULL ) {
// if failed to get semaphore then just quietly fail
if( !sem->get(this) ) {
return;
}
if (!_spi_sem->take_nonblocking()) {
return;
}
_spi->cs_assert();
@ -149,11 +141,8 @@ void AP_OpticalFlow_ADNS3080::write_register(uint8_t address, uint8_t value)
_spi->transfer(value);
_spi->cs_release();
// get spi3 semaphore if required
if( sem != NULL ) {
sem->release(this);
}
_spi_sem->give();
}
// 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
AP_HAL::SPIDeviceDriver *_spi;
AP_HAL::Semaphore *_spi_sem;
};
#endif