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

OptFlow: initialisation fix 

Thanks to DongFang (aka Soren) for finding the issue and the solution
This commit is contained in:
Randy Mackay 2013-07-11 14:14:37 +09:00
parent 5ae62f4f04
commit 012d7b0c0a
2 changed files with 26 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp
View File

@ -93,11 +93,6 @@ finish:
// 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
@ -110,9 +105,18 @@ finish:
// Read a register from the sensor
uint8_t AP_OpticalFlow_ADNS3080::read_register(uint8_t address)
{
if (_spi == NULL) return 0;
AP_HAL::Semaphore *spi_sem;
if (!_spi_sem->take_nonblocking()) {
// check that we have an spi bus
if (_spi == NULL) {
return 0;
}
// get spi bus semaphore
spi_sem = _spi->get_semaphore();
// try to get control of the spi bus
if (spi_sem == NULL || !spi_sem->take_nonblocking()) {
return 0;
}
@ -125,7 +129,8 @@ uint8_t AP_OpticalFlow_ADNS3080::read_register(uint8_t address)
_spi->cs_release();
_spi_sem->give();
// release the spi bus
spi_sem->give();
return result;
}
@ -133,9 +138,18 @@ uint8_t AP_OpticalFlow_ADNS3080::read_register(uint8_t address)
// write a value to one of the sensor's registers
void AP_OpticalFlow_ADNS3080::write_register(uint8_t address, uint8_t value)
{
if (_spi == NULL) return;
AP_HAL::Semaphore *spi_sem;
if (!_spi_sem->take_nonblocking()) {
// check that we have an spi bus
if (_spi == NULL) {
return;
}
// get spi bus semaphore
spi_sem = _spi->get_semaphore();
// try to get control of the spi bus
if (spi_sem == NULL || !spi_sem->take_nonblocking()) {
return;
}
@ -149,7 +163,8 @@ void AP_OpticalFlow_ADNS3080::write_register(uint8_t address, uint8_t value)
_spi->cs_release();
_spi_sem->give();
// release the spi bus
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

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