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 aec1a24bda..110ab55173 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 @@ -21,7 +21,7 @@ FastSerialPort0(Serial); // FTDI/console AP_OpticalFlow_ADNS3080 flowSensor; -//AP_OpticalFlow_ADNS3080 flowSensor(A6); // override chip select pin to use A6 if using APM2 +//AP_OpticalFlow_ADNS3080 flowSensor(A3); // override chip select pin to use A3 if using APM2 void setup() { @@ -110,7 +110,6 @@ void display_config() void set_frame_rate() { int value; - byte extConfig; // frame rate Serial.print("frame rate: "); @@ -196,7 +195,6 @@ void display_image_continuously() void set_resolution() { int value; - byte reg; int resolution = flowSensor.get_resolution(); Serial.print("resolution: "); Serial.println(resolution); @@ -231,7 +229,6 @@ void set_resolution() void set_shutter_speed() { int value; - byte extConfig; // shutter speed Serial.print("shutter speed: "); @@ -300,7 +297,6 @@ void set_shutter_speed() // void display_motion() { - int value; boolean first_time = true; Serial.flush(); @@ -317,20 +313,18 @@ void display_motion() Serial.println("overflow!!"); // x,y,squal - //if( flowSensor.motion() || first_time ) { - Serial.print("x/dx: "); - Serial.print(flowSensor.x,DEC); - Serial.print("/"); - Serial.print(flowSensor.dx,DEC); - Serial.print("\ty/dy: "); - Serial.print(flowSensor.y,DEC); - Serial.print("/"); - Serial.print(flowSensor.dy,DEC); - Serial.print("\tsqual:"); - Serial.print(flowSensor.surface_quality,DEC); - Serial.println(); - first_time = false; - //} + Serial.print("x/dx: "); + Serial.print(flowSensor.x,DEC); + Serial.print("/"); + Serial.print(flowSensor.dx,DEC); + Serial.print("\ty/dy: "); + Serial.print(flowSensor.y,DEC); + Serial.print("/"); + Serial.print(flowSensor.dy,DEC); + Serial.print("\tsqual:"); + Serial.print(flowSensor.surface_quality,DEC); + Serial.println(); + first_time = false; // short delay delay(100);