diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp index b4fb52ddd9..f3e377cdb5 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp @@ -33,6 +33,17 @@ #define AP_SPI_TIMEOUT 1000 +// We use Serial Port 2 in SPI Mode +#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + #define AP_SPI_DATAIN 50 // MISO // PB3 + #define AP_SPI_DATAOUT 51 // MOSI // PB2 + #define AP_SPI_CLOCK 52 // SCK // PB1 +#else // normal arduino SPI pins...these need to be checked + #define AP_SPI_DATAIN 12 //MISO + #define AP_SPI_DATAOUT 11 //MOSI + #define AP_SPI_CLOCK 13 //SCK +#endif + union NumericIntType { int intValue; diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h index fc998e139f..60a26f57d9 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h @@ -3,6 +3,10 @@ #include "AP_OpticalFlow.h" +// default pin settings +#define ADNS3080_CHIP_SELECT 34 // PC3 +#define ADNS3080_RESET 0 // reset pin is unattached by default + // orientations for ADNS3080 sensor #define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD ROTATION_YAW_180 #define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD_RIGHT ROTATION_YAW_135 @@ -19,21 +23,6 @@ // scaler - value returned when sensor is moved equivalent of 1 pixel #define AP_OPTICALFLOW_ADNS3080_SCALER 1.1 -// We use Serial Port 2 in SPI Mode -#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) - #define AP_SPI_DATAIN 50 // MISO // PB3 - #define AP_SPI_DATAOUT 51 // MOSI // PB2 - #define AP_SPI_CLOCK 52 // SCK // PB1 - #define ADNS3080_CHIP_SELECT 34 // PC3 - #define ADNS3080_RESET 0 // reset pin is unattached by default -#else // normal arduino SPI pins...these need to be checked - #define AP_SPI_DATAIN 12 //MISO - #define AP_SPI_DATAOUT 11 //MOSI - #define AP_SPI_CLOCK 13 //SCK - #define ADNS3080_CHIP_SELECT 10 //SS - #define ADNS3080_RESET 9 //RESET -#endif - // ADNS3080 hardware config #define ADNS3080_PIXELS_X 30 #define ADNS3080_PIXELS_Y 30 @@ -85,7 +74,6 @@ #define ADNS3080_FRAME_RATE_MAX 6469 #define ADNS3080_FRAME_RATE_MIN 2000 - class AP_OpticalFlow_ADNS3080 : public AP_OpticalFlow { private: