static uint16_t read_val12(spi_device dev, adxl362_axis_t axis)
{
adxl362_reg_t reg_h, reg_l;
uint8_t val_h, val_l;
switch (axis) {
case ADXL362_AXIS_X:
reg_h = ADXL362_REG_XDATA_H;
reg_l = ADXL362_REG_XDATA_L;
break;
case ADXL362_AXIS_Y:
reg_h = ADXL362_REG_YDATA_H;
reg_l = ADXL362_REG_YDATA_L;
break;
case ADXL362_AXIS_Z:
reg_h = ADXL362_REG_ZDATA_H;
reg_l = ADXL362_REG_ZDATA_L;
break;
default:
return 0xFFFF;
}
val_h = read_register(dev, reg_h);
val_l = read_register(dev, reg_l);
return create_value12(val_h, val_l);
}
// read latest values from sensor and fill in x,y and totals
void
AP_OpticalFlow_ADNS3080::update(uint32_t now)
{
uint8_t motion_reg;
surface_quality = read_register(ADNS3080_SQUAL);
hal.scheduler->delay_microseconds(50);
// check for movement, update x,y values
motion_reg = read_register(ADNS3080_MOTION);
// check if we've had an overflow
_overflow = ((motion_reg & 0x10) != 0);
if( (motion_reg & 0x80) != 0 ) {
raw_dx = ((int8_t)read_register(ADNS3080_DELTA_X));
hal.scheduler->delay_microseconds(50);
raw_dy = ((int8_t)read_register(ADNS3080_DELTA_Y));
_motion = true;
}else{
raw_dx = 0;
raw_dy = 0;
}
last_update = hal.scheduler->millis();
apply_orientation_matrix();
}
// set_shutter_speed_auto - set shutter speed to auto (true), or manual (false)
void
AP_OpticalFlow_ADNS3080::set_shutter_speed(unsigned int shutter_speed)
{
NumericIntType aNum;
aNum.uintValue = shutter_speed;
// set shutter speed to manual
set_shutter_speed_auto(false);
delayMicroseconds(50); // small delay
// set specific shutter speed
write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,aNum.byteValue[0]);
delayMicroseconds(50); // small delay
write_register(ADNS3080_SHUTTER_MAX_BOUND_UPPER,aNum.byteValue[1]);
delayMicroseconds(50); // small delay
// larger delay
delay(50);
// need to update frame period to cause shutter value to take effect
aNum.byteValue[1] = read_register(ADNS3080_FRAME_PERIOD_UPPER);
delayMicroseconds(50); // small delay
aNum.byteValue[0] = read_register(ADNS3080_FRAME_PERIOD_LOWER);
delayMicroseconds(50); // small delay
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,aNum.byteValue[0]);
delayMicroseconds(50); // small delay
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,aNum.byteValue[1]);
delayMicroseconds(50); // small delay
}
// read latest values from sensor and fill in x,y and totals
bool
AP_OpticalFlow_ADNS3080::update()
{
byte motion_reg;
surface_quality = (unsigned int)read_register(ADNS3080_SQUAL);
delayMicroseconds(50); // small delay
// check for movement, update x,y values
motion_reg = read_register(ADNS3080_MOTION);
_overflow = ((motion_reg & 0x10) != 0); // check if we've had an overflow
if( (motion_reg & 0x80) != 0 ) {
raw_dx = ((char)read_register(ADNS3080_DELTA_X));
delayMicroseconds(50); // small delay
raw_dy = ((char)read_register(ADNS3080_DELTA_Y));
_motion = true;
}else{
raw_dx = 0;
raw_dy = 0;
}
last_update = millis();
apply_orientation_matrix();
return true;
}
int em8300_setregblock(struct em8300_s *em, int offset, int val, int len)
{
int i;
for (i = 1000; i; i--) {
if (!read_register(0x1c1a)) {
break;
}
if (!i) {
return -ETIME;
}
}
#if 0 /* FIXME: was in the zeev01 branch, verify if it is necessary */
val = val | (val << 8) | (val << 16) | (val << 24);
#endif
writel(offset & 0xffff, &em->mem[0x1c11]);
writel((offset >> 16) & 0xffff, &em->mem[0x1c12]);
writel(len, &em->mem[0x1c13]);
writel(len, &em->mem[0x1c14]);
writel(0, &em->mem[0x1c15]);
writel(1, &em->mem[0x1c16]);
writel(1, &em->mem[0x1c17]);
writel(offset & 0xffff, &em->mem[0x1c18]);
writel(0, &em->mem[0x1c19]);
writel(1, &em->mem[0x1c1a]);
for (i = 0; i < len / 4; i++) {
writel(val, &em->mem[0x11800]);
}
switch (len % 4) {
case 1:
writel(val, &em->mem[0x10000]);
break;
case 2:
writel(val, &em->mem[0x10800]);
break;
case 3:
writel(val, &em->mem[0x11000]);
break;
}
for (i = 1000; i; i--) {
if (!read_register(0x1c1a)) {
break;
}
if (!i) {
return -ETIME;
}
}
#if 0 /* FIXME: was in zeev01 branch, verify if it is necessary */
if (em8300_waitfor(em, 0x1c1a, 0, 1))
return -ETIME;
#endif
return 0;
}
// get_shutter_speed_auto - returns true if shutter speed is adjusted
// automatically, false if manual
uint16_t AP_OpticalFlow_ADNS3080::get_shutter_speed()
{
NumericIntType aNum;
aNum.byteValue[1] = read_register(ADNS3080_SHUTTER_UPPER);
hal.scheduler->delay_microseconds(50);
aNum.byteValue[0] = read_register(ADNS3080_SHUTTER_LOWER);
return aNum.uintValue;
}
// get frame period
uint16_t AP_OpticalFlow_ADNS3080::get_frame_period()
{
NumericIntType aNum;
aNum.byteValue[1] = read_register(ADNS3080_FRAME_PERIOD_UPPER);
hal.scheduler->delay_microseconds(50);
aNum.byteValue[0] = read_register(ADNS3080_FRAME_PERIOD_LOWER);
return aNum.uintValue;
}
void Radio::startListening(void) {
write_register(CONFIG, read_register(CONFIG) | _BV(PRIM_RX));
write_register(STATUS, _BV(RX_DR) | _BV(TX_DS) | _BV(MAX_RT));
if (read_register(FEATURE) & _BV(EN_ACK_PAY)) {
flush_tx();
}
}
irom io_error_t io_mcp_get_pin_info(string_t *dst, const struct io_info_entry_T *info, io_data_pin_entry_t *pin_data, const io_config_pin_entry_t *pin_config, int pin)
{
int bank, bankpin, tv;
int io, olat, cached;
mcp_data_pin_t *mcp_pin_data;
bank = (pin & 0x08) >> 3;
bankpin = pin & 0x07;
mcp_pin_data = &mcp_data_pin_table[info->instance][pin];
switch(pin_config->llmode)
{
case(io_pin_ll_input_analog):
{
if(read_register(dst, info->address, GPIO(bank), &tv) != io_ok)
return(io_error);
string_format(dst, "current io: %s", onoff(tv & (1 << bankpin)));
break;
}
case(io_pin_ll_counter):
{
if(read_register(dst, info->address, GPIO(bank), &tv) != io_ok)
return(io_error);
string_format(dst, "current io: %s, debounce: %d", onoff(tv & (1 << bankpin)), mcp_pin_data->debounce);
break;
}
case(io_pin_ll_output_digital):
{
if(read_register(dst, info->address, GPIO(bank), &tv) != io_ok)
return(io_error);
io = tv & (1 << bankpin);
if(read_register(dst, info->address, OLAT(bank), &tv) != io_ok)
return(io_error);
olat = tv & (1 << bankpin);
cached = pin_output_cache[bank] & (1 << bankpin);
string_format(dst, "current latch: %s, io: %s, cache: %s", onoff(io), onoff(olat), onoff(cached));
break;
}
default:
{
}
}
return(io_ok);
}
// get frame period
unsigned int
AP_OpticalFlow_ADNS3080::get_frame_period()
{
NumericIntType aNum;
aNum.byteValue[1] = read_register(ADNS3080_FRAME_PERIOD_UPPER);
delayMicroseconds(50); // small delay
aNum.byteValue[0] = read_register(ADNS3080_FRAME_PERIOD_LOWER);
return aNum.uintValue;
}
// get_shutter_speed_auto - returns true if shutter speed is adjusted automatically, false if manual
unsigned int
AP_OpticalFlow_ADNS3080::get_shutter_speed()
{
NumericIntType aNum;
aNum.byteValue[1] = read_register(ADNS3080_SHUTTER_UPPER);
delayMicroseconds(50); // small delay
aNum.byteValue[0] = read_register(ADNS3080_SHUTTER_LOWER);
return aNum.uintValue;
}
// Public Methods //////////////////////////////////////////////////////////////
// init - initialise sensor
// assumes SPI bus has been initialised but will attempt to initialise
// nonstandard SPI3 bus if required
bool
AP_OpticalFlow_ADNS3080::init()
{
int8_t retry = 0;
bool retvalue = false;
// suspend timer while we set-up SPI communication
hal.scheduler->suspend_timer_procs();
if( _reset_pin != 0)
hal.gpio->pinMode(_reset_pin, GPIO_OUTPUT);
// reset the device
reset();
// check 3 times for the sensor on standard SPI bus
_spi = hal.spi->device(AP_HAL::SPIDevice_ADNS3080_SPI0);
if (_spi == NULL) {
retvalue = false; goto finish;
}
while( retvalue == false && retry < 3 ) {
if( read_register(ADNS3080_PRODUCT_ID) == 0x17 ) {
retvalue = true;
goto finish;
}
retry++;
}
// if not found, check 3 times on SPI3
_spi = hal.spi->device(AP_HAL::SPIDevice_ADNS3080_SPI3);
if (_spi == NULL) {
retvalue = false; goto finish;
}
retry = 0;
while( retvalue == false && retry < 3 ) {
if( read_register(ADNS3080_PRODUCT_ID) == 0x17 ) {
retvalue = true;
}
retry++;
}
// If we fail to find on SPI3, no connection available.
retvalue = false;
_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 );
}
return retvalue;
}
void
single_step ()
{
branch_type br, isannulled();
CORE_ADDR pc;
long pc_instruction;
if (!one_stepped)
{
/* Always set breakpoint for NPC. */
next_pc = read_register (NPC_REGNUM);
npc4 = next_pc + 4; /* branch not taken */
target_insert_breakpoint (next_pc, break_mem[0]);
/* printf ("set break at %x\n",next_pc); */
pc = read_register (PC_REGNUM);
pc_instruction = read_memory_integer (pc, sizeof(pc_instruction));
br = isannulled (pc_instruction, pc, &target);
brknpc4 = brktrg = 0;
if (br == bicca)
{
/* Conditional annulled branch will either end up at
npc (if taken) or at npc+4 (if not taken).
Trap npc+4. */
brknpc4 = 1;
target_insert_breakpoint (npc4, break_mem[1]);
}
else if (br == baa && target != next_pc)
{
/* Unconditional annulled branch will always end up at
the target. */
brktrg = 1;
target_insert_breakpoint (target, break_mem[2]);
}
/* We are ready to let it go */
one_stepped = 1;
return;
}
else
{
/* Remove breakpoints */
target_remove_breakpoint (next_pc, break_mem[0]);
if (brknpc4)
target_remove_breakpoint (npc4, break_mem[1]);
if (brktrg)
target_remove_breakpoint (target, break_mem[2]);
one_stepped = 0;
}
}
params read_params()
{
params read_params;
read_params.feedback = read_register(FEEDBACK_ADDR);
read_params.commanded_freq = read_register(COMMANDED_FREQ_ADDR);
read_params.output_current = read_register(OUTPUT_CURRENT_ADDR);
read_params.output_voltage = read_register(OUTPUT_VOLTAGE_ADDR);
return read_params;
}
int Sonar::read() {
if(!this->has_init) {
init();
}
set_mode(SINGLE_SHOT);
if(sonar_mode == METRIC) {
return read_register(RESULT_1);
}
else {
return ( (read_register(RESULT_1)) * 3937)/1000;
}
}
int ni_tio_rinsn(struct ni_gpct *counter, struct comedi_insn *insn,
unsigned int *data)
{
struct ni_gpct_device *counter_dev = counter->counter_dev;
const unsigned channel = CR_CHAN(insn->chanspec);
unsigned first_read;
unsigned second_read;
unsigned correct_read;
if (insn->n < 1)
return 0;
switch (channel) {
case 0:
ni_tio_set_bits(counter,
NITIO_Gi_Command_Reg(counter->counter_index),
Gi_Save_Trace_Bit, 0);
ni_tio_set_bits(counter,
NITIO_Gi_Command_Reg(counter->counter_index),
Gi_Save_Trace_Bit, Gi_Save_Trace_Bit);
/* The count doesn't get latched until the next clock edge, so it is possible the count
may change (once) while we are reading. Since the read of the SW_Save_Reg isn't
atomic (apparently even when it's a 32 bit register according to 660x docs),
we need to read twice and make sure the reading hasn't changed. If it has,
a third read will be correct since the count value will definitely have latched by then. */
first_read =
read_register(counter,
NITIO_Gi_SW_Save_Reg(counter->counter_index));
second_read =
read_register(counter,
NITIO_Gi_SW_Save_Reg(counter->counter_index));
if (first_read != second_read)
correct_read =
read_register(counter,
NITIO_Gi_SW_Save_Reg(counter->
counter_index));
else
correct_read = first_read;
data[0] = correct_read;
return 0;
break;
case 1:
data[0] =
counter_dev->
regs[NITIO_Gi_LoadA_Reg(counter->counter_index)];
break;
case 2:
data[0] =
counter_dev->
regs[NITIO_Gi_LoadB_Reg(counter->counter_index)];
break;
}
return 0;
}
void Radio::stopListening(void) {
if (read_register(FEATURE) & _BV(EN_ACK_PAY)) {
_delay_us(155);
flush_tx();
}
write_register(CONFIG, (read_register(CONFIG)) & ~_BV(PRIM_RX));
// for 3 pins solution TX mode is only left with additional powerDown/powerUp cycle.
powerDown();
powerUp();
}
static CORE_ADDR
i386lynx_saved_pc_after_call (struct frame_info *frame)
{
char opcode[7];
static const unsigned char call_inst[] =
{ 0x9a, 0, 0, 0, 0, 8, 0 }; /* lcall 0x8,0x0 */
read_memory_nobpt (frame->pc - 7, opcode, 7);
if (memcmp (opcode, call_inst, 7) == 0)
return read_memory_unsigned_integer (read_register (SP_REGNUM) + 4, 4);
return read_memory_unsigned_integer (read_register (SP_REGNUM), 4);
}
int lem_interrupt(void)
{
uint16_t cycles = 0;
uint16_t b = read_register(REG_B);
switch (read_register(REG_A)) {
case 0:
if (b) {
if (connected == 0) {
clock_gettime(CLOCK_REALTIME, &tp_prev);
// TODO don't start up for ~1s
}
connected = 1;
vram = b;
} else {
connected = 0;
}
break;
case 1:
if (b) {
custom_font = 1;
font_ram = b;
} else {
custom_font = 1;
}
break;
case 2:
if (b) {
custom_pal = 1;
pal_ram = b;
} else {
custom_pal = 1;
}
break;
case 3:
border_col = b;
break;
case 4:
write_memory(b, FONT_LEN, font);
cycles = 256;
break;
case 5:
write_memory(b, PAL_LEN, pal);
cycles = 16;
break;
default:
break;
}
return cycles;
}
bool Radio::setup(void) {
HalfDuplexSPI::setup();
csnHigh();
// Must allow the radio time to settle else configuration bits will not necessarily stick.
// This is actually only required following power up but some settling time also appears to
// be required after resets too. For full coverage, we'll always assume the worst.
// Enabling 16b CRC is by far the most obvious case if the wrong timing is used - or skipped.
// Technically we require 4.5ms + 14us as a worst case. We'll just call it 5ms for good measure.
// WARNING: Delay is based on P-variant whereby non-P *may* require different timing.
_delay_ms(5);
// Reset CONFIG and enable 16-bit CRC.
write_register(CONFIG, 0 | _BV(EN_CRC) | _BV(CRCO));
// Set 1500uS (minimum for 32B payload in ESB@250KBPS) timeouts, to make testing a little easier
// WARNING: If this is ever lowered, either 250KBS mode with AA is broken or maximum packet
// sizes must never be used. See documentation for a more complete explanation.
setRetries(5, 15);
uint8_t setup = read_register(RF_SETUP);
// Then set the data rate to the slowest (and most reliable) speed supported by all
// hardware.
setDataRate(DataRate::RATE_1MBPS);
write_register(FEATURE, 0);
write_register(DYNPD, 0);
// Reset current status
// Notice reset and flush is the last thing we do
write_register(STATUS, _BV(RX_DR) | _BV(TX_DS) | _BV(MAX_RT));
setChannel(76);
// Flush buffers
flush_rx();
flush_tx();
//Power up by default when setup() is called.
powerUp();
// Enable PTX, do not write CE high so radio will remain in standby I mode ( 130us max to transition to RX or TX
// instead of 1500us from powerUp ) PTX should use only 22uA of power.
write_register(CONFIG, (read_register(CONFIG)) & ~_BV(PRIM_RX));
// If setup is 0 or ff then there was no response from module.
return setup != 0 && setup != 0xff;
}
int ni_tio_rinsn(struct ni_gpct *counter, struct comedi_insn *insn,
unsigned int *data)
{
struct ni_gpct_device *counter_dev = counter->counter_dev;
const unsigned channel = CR_CHAN(insn->chanspec);
unsigned first_read;
unsigned second_read;
unsigned correct_read;
if (insn->n < 1)
return 0;
switch (channel) {
case 0:
ni_tio_set_bits(counter,
NITIO_Gi_Command_Reg(counter->counter_index),
Gi_Save_Trace_Bit, 0);
ni_tio_set_bits(counter,
NITIO_Gi_Command_Reg(counter->counter_index),
Gi_Save_Trace_Bit, Gi_Save_Trace_Bit);
first_read =
read_register(counter,
NITIO_Gi_SW_Save_Reg(counter->counter_index));
second_read =
read_register(counter,
NITIO_Gi_SW_Save_Reg(counter->counter_index));
if (first_read != second_read)
correct_read =
read_register(counter,
NITIO_Gi_SW_Save_Reg(counter->
counter_index));
else
correct_read = first_read;
data[0] = correct_read;
return 0;
break;
case 1:
data[0] =
counter_dev->
regs[NITIO_Gi_LoadA_Reg(counter->counter_index)];
break;
case 2:
data[0] =
counter_dev->
regs[NITIO_Gi_LoadB_Reg(counter->counter_index)];
break;
};
return 0;
}
float MAX44009::get_lux(void)
{
int luxHigh = read_register(0x03);
int luxLow = read_register(0x04);
int exponent = (luxHigh & 0xf0) >> 4;
int mant = (luxHigh & 0x0f) << 4 | luxLow;
return (float)(((0x00000001 << exponent) * (float)mant) * 0.045);
//return (float)((pow(2,exponent) * mant) * 0.045);
}
void set_time(struct tm *t)
{
int regA, regB;
if (Wflag) {
/* Set A and B registers to their proper values according to the AT
* reference manual. (For if it gets messed up, but the BIOS doesn't
* repair it.)
*/
write_register(RTC_REG_A, RTC_A_DV_OK | RTC_A_RS_DEF);
write_register(RTC_REG_B, RTC_B_24);
}
/* Inhibit updates. */
regB= read_register(RTC_REG_B);
write_register(RTC_REG_B, regB | RTC_B_SET);
t->tm_mon++; /* Counts from 1. */
if (y2kflag) {
/* Set the clock back 20 years to avoid Y2K bug, good until 2020. */
if (t->tm_year >= 100) t->tm_year -= 20;
}
if ((regB & 0x04) == 0) {
/* Convert binary to BCD (default RTC mode) */
t->tm_year = dec_to_bcd(t->tm_year % 100);
t->tm_mon = dec_to_bcd(t->tm_mon);
t->tm_mday = dec_to_bcd(t->tm_mday);
t->tm_hour = dec_to_bcd(t->tm_hour);
t->tm_min = dec_to_bcd(t->tm_min);
t->tm_sec = dec_to_bcd(t->tm_sec);
}
write_register(RTC_YEAR, t->tm_year);
write_register(RTC_MONTH, t->tm_mon);
write_register(RTC_MDAY, t->tm_mday);
write_register(RTC_HOUR, t->tm_hour);
write_register(RTC_MIN, t->tm_min);
write_register(RTC_SEC, t->tm_sec);
/* Stop the clock. */
regA= read_register(RTC_REG_A);
write_register(RTC_REG_A, regA | RTC_A_DV_STOP);
/* Allow updates and restart the clock. */
write_register(RTC_REG_B, regB);
write_register(RTC_REG_A, regA);
}
/*
* Reads current pressure value
*/
long SCP1000::pressure()
{
unsigned long pressure;
// Pressure value is in 19-bit unsigned format
// Value = Pa * 4
pressure = read_register(PRESSURE, 1); // Read MSB
pressure &= 0b00000111; // mask unused bits
pressure <<= 16; // shift into upper word
pressure |= read_register(PRESSURE_LSB, 2); // read low word
// Convert to real pressure in hPa
return pressure>>2;
}
void
ecompass_init(void)
{
DECO("Starting ecompass init...");
write_register(CTRL_REG1, 0x37);
transfer(ECOMPASS_ID);
// Acceleromter data ready on int1
//write_register(CTRL_REG3, 1 << 2);
// Magnometer data ready on int2
//write_register(CTRL_REG4, 1 << 2);
write_register(CTRL_REG5, 0x8);
transfer(ECOMPASS_ID);
write_register(CTRL_REG7, 0x0);
transfer(ECOMPASS_ID);
udelay(400000);
int ret;
int ret_i = read_register(WHO_AM_I_REG | READ_MASK);
transfer(ECOMPASS_ID);
if ((ret = get_data(ret_i)) != WHO_AM_I) {
DECO("E-Comapss initilisation failed (who_am_i is %d)\n", ret);
} else {
DECO("Init success!\n");
}
//udelay(500000);
//calibrate();
}
void Radio::openWritingPipe(const uint8_t *address) {
// Note that AVR 8-bit uC's store this LSB first, and the NRF24L01(+) expects it LSB first too, so we're good.
write_register(RX_ADDR_P0, address, ADDRESS_WIDTH);
write_register(TX_ADDR, address, ADDRESS_WIDTH);
write_register(RX_PW_P0, PAYLOAD_SIZE);
write_register(EN_RXADDR, read_register(EN_RXADDR) | _BV(ERX_P0));
}
/** @brief Read raw measurement */
int8_t HalMagHMC5883L::readRaw(math::Vector3i& compassMeas_U)
{
uint8_t buff[6];
uint8_t regValue;
/* Check if data is ready */
if (!read_register(REG_ADDR_SR, ®Value))
{
return -1;
}
/* Check that status is ready */
if (!(regValue & REG_MASK_SR_RDY))
{
return -2;
}
/* Read data */
uint8_t status = _bus.read(I2C_DEV_ADD_HMC5883L, REG_ADDR_DORXM, 6, buff);
if (status != 0)
{
return -3;
}
compassMeas_U(
- (int16_t) (((uint16_t)(buff[4]) << 8) | ((uint16_t) buff[5])), // -Y
- (int16_t) (((uint16_t)(buff[0]) << 8) | ((uint16_t) buff[1])), // -X
- (int16_t) (((uint16_t)(buff[2]) << 8) | ((uint16_t) buff[3]))); // -Z
return 0;
}
// Public Methods //////////////////////////////////////////////////////////////
// init - initialise sensor
// initCommAPI parameter controls whether SPI interface is initialised (set to false if other devices are on the SPI bus and have already initialised the interface)
bool
AP_OpticalFlow_ADNS3080::init(bool initCommAPI)
{
int retry = 0;
pinMode(AP_SPI_DATAOUT,OUTPUT);
pinMode(AP_SPI_DATAIN,INPUT);
pinMode(AP_SPI_CLOCK,OUTPUT);
pinMode(_cs_pin,OUTPUT);
if( _reset_pin != 0)
pinMode(ADNS3080_RESET,OUTPUT);
digitalWrite(_cs_pin,HIGH); // disable device (Chip select is active low)
// reset the device
reset();
// start the SPI library:
if( initCommAPI ) {
SPI.begin();
}
// check the sensor is functioning
while( retry < 3 ) {
if( read_register(ADNS3080_PRODUCT_ID) == 0x17 )
return true;
retry++;
}
return false;
}
// get_pixel_data - captures an image from the sensor and stores it to the pixe_data array
void
AP_OpticalFlow_ADNS3080::print_pixel_data(Stream *serPort)
{
int i,j;
bool isFirstPixel = true;
byte regValue;
byte pixelValue;
// write to frame capture register to force capture of frame
write_register(ADNS3080_FRAME_CAPTURE,0x83);
// wait 3 frame periods + 10 nanoseconds for frame to be captured
delayMicroseconds(1510); // min frame speed is 2000 frames/second so 1 frame = 500 nano seconds. so 500 x 3 + 10 = 1510
// display the pixel data
for( i=0; i<ADNS3080_PIXELS_Y; i++ ) {
for( j=0; j<ADNS3080_PIXELS_X; j++ ) {
regValue = read_register(ADNS3080_FRAME_CAPTURE);
if( isFirstPixel && (regValue & 0x40) == 0 ) {
serPort->println("failed to find first pixel");
}
isFirstPixel = false;
pixelValue = ( regValue << 2);
serPort->print(pixelValue,DEC);
if( j!= ADNS3080_PIXELS_X-1 )
serPort->print(",");
delayMicroseconds(50);
}
serPort->println();
}
// hardware reset to restore sensor to normal operation
reset();
}
// returns resolution (either 400 or 1600 counts per inch)
int16_t AP_OpticalFlow_ADNS3080::get_resolution()
{
if( (read_register(ADNS3080_CONFIGURATION_BITS) & 0x10) == 0 )
return 400;
else
return 1600;
}
