From f096d8cb2369c6906983935d80920bbd06f26d3e Mon Sep 17 00:00:00 2001 From: "rmackay9@yahoo.com" Date: Sun, 31 Jul 2011 04:31:36 +0000 Subject: [PATCH] OpticalFlow - add check to see if sensor has initialised correctly. git-svn-id: https://arducopter.googlecode.com/svn/trunk@2974 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- .../AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp | 14 +++++++++++--- .../AP_OpticalFlow_test/AP_OpticalFlow_test.pde | 4 +++- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp index 71cd7110f3..efebb7c6ff 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp @@ -51,6 +51,8 @@ AP_OpticalFlow_ADNS3080::AP_OpticalFlow_ADNS3080() 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); @@ -65,10 +67,16 @@ AP_OpticalFlow_ADNS3080::init(bool initCommAPI) // start the SPI library: if( initCommAPI ) { SPI.begin(); - //SPI.setBitOrder(MSBFIRST); - //SPI.setDataMode(SPI_MODE3); - //SPI.setClockDivider(SPI_CLOCK_DIV8); // sensor running at 2Mhz. this is it's maximum speed } + + // check the sensor is functioning + if( retry < 3 ) { + if( read_register(ADNS3080_PRODUCT_ID) == 0x17 ) + return true; + else + retry++; + }else + return false; } // diff --git a/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde b/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde index aca9007132..3021ed5d09 100644 --- a/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde +++ b/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde @@ -16,7 +16,9 @@ void setup() delay(1000); - flowSensor.init(); // flowSensor initialization + // flowSensor initialization + if( flowSensor.init() == false ) + Serial.println("Failed to initialise ADNS3080"); flowSensor.set_orientation(AP_OPTICALFLOW_ADNS3080_PINS_FORWARD); flowSensor.set_field_of_view(AP_OPTICALFLOW_ADNS3080_12_FOV);