// Read a register from the sensor byte AP_OpticalFlow_ADNS3080::read_register(byte address) { byte result = 0, junk = 0; backup_spi_settings(); // take the chip select low to select the device digitalWrite(_cs_pin, LOW); // send the device the register you want to read: junk = SPI.transfer(address); // small delay delayMicroseconds(50); // send a value of 0 to read the first byte returned: result = SPI.transfer(0x00); // take the chip select high to de-select: digitalWrite(_cs_pin, HIGH); restore_spi_settings(); return result; }
// Read a register from the sensor byte AP_OpticalFlow_ADNS3080::read_register(byte address) { uint8_t result = 0; uint8_t junk = 0; // get spi semaphore if required if( _spi_semaphore != NULL ) { // if failed to get semaphore then just quietly fail if( !_spi_semaphore->get(this) ) { return 0; } } backup_spi_settings(); // take the chip select low to select the device digitalWrite(_cs_pin, LOW); if( _spi_bus == ADNS3080_SPIBUS_1 ) { junk = SPI.transfer(address); // send the device the register you want to read: delayMicroseconds(50); // small delay result = SPI.transfer(0x00); // send a value of 0 to read the first byte returned: }else if( _spi_bus == ADNS3080_SPIBUS_3 ) { junk = SPI3.transfer(address); // send the device the register you want to read: delayMicroseconds(50); // small delay result = SPI3.transfer(0x00); // send a value of 0 to read the first byte returned: } // take the chip select high to de-select: digitalWrite(_cs_pin, HIGH); restore_spi_settings(); // get spi semaphore if required if( _spi_semaphore != NULL ) { _spi_semaphore->release(this); } return result; }
// write a value to one of the sensor's registers void AP_OpticalFlow_ADNS3080::write_register(byte address, byte value){ byte junk = 0; backup_spi_settings(); // take the chip select low to select the device digitalWrite(_cs_pin, LOW); // send register address junk = SPI.transfer(address | 0x80); // small delay delayMicroseconds(50); // send data junk = SPI.transfer(value); // take the chip select high to de-select: digitalWrite(_cs_pin, HIGH); restore_spi_settings(); }
// write a value to one of the sensor's registers void AP_OpticalFlow_ADNS3080::write_register(byte address, byte value) { byte junk = 0; // get spi semaphore if required if( _spi_semaphore != NULL ) { // if failed to get semaphore then just quietly fail if( !_spi_semaphore->get(this) ) { return; } } backup_spi_settings(); // take the chip select low to select the device digitalWrite(_cs_pin, LOW); if( _spi_bus == ADNS3080_SPIBUS_1 ) { junk = SPI.transfer(address | 0x80 ); // send register address delayMicroseconds(50); // small delay junk = SPI.transfer(value); // send data }else if( _spi_bus == ADNS3080_SPIBUS_3 ) { junk = SPI3.transfer(address | 0x80 ); // send register address delayMicroseconds(50); // small delay junk = SPI3.transfer(value); // send data } // take the chip select high to de-select: digitalWrite(_cs_pin, HIGH); restore_spi_settings(); // get spi3 semaphore if required if( _spi_semaphore != NULL ) { _spi_semaphore->release(this); } }