OptFlow: update example sketch to work with shrunken library
This commit is contained in:
Randy Mackay
parent
8caa5159f8
commit
3823eed865
@ -14,7 +14,7 @@
|
||||
|
||||
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
|
||||
|
||||
AP_OpticalFlow_ADNS3080 flowSensor;
|
||||
AP_OpticalFlow_ADNS3080 optflow;
|
||||
|
||||
void setup()
|
||||
{
|
||||
@ -23,13 +23,11 @@ void setup()
|
||||
hal.scheduler->delay(1000);
|
||||
|
||||
// flowSensor initialization
|
||||
if( flowSensor.init() == false ) {
|
||||
optflow.init();
|
||||
if (!optflow.healthy()) {
|
||||
hal.console->print("Failed to initialise ADNS3080 ");
|
||||
}
|
||||
|
||||
flowSensor.set_orientation(AP_OPTICALFLOW_ADNS3080_PINS_FORWARD);
|
||||
flowSensor.set_field_of_view(AP_OPTICALFLOW_ADNS3080_08_FOV);
|
||||
|
||||
hal.scheduler->delay(1000);
|
||||
}
|
||||
|
||||
@ -41,14 +39,9 @@ void display_menu()
|
||||
hal.console->println();
|
||||
hal.console->println("please choose from the following options:");
|
||||
hal.console->println(" c - display all config");
|
||||
hal.console->println(" f - set frame rate");
|
||||
hal.console->println(" i - display image");
|
||||
hal.console->println(" I - display image continuously");
|
||||
hal.console->println(" m - display motion");
|
||||
hal.console->println(" r - set resolution");
|
||||
hal.console->println(" s - set shutter speed");
|
||||
hal.console->println(" z - clear all motion");
|
||||
hal.console->println(" a - frame rate auto/manual");
|
||||
hal.console->println();
|
||||
}
|
||||
|
||||
@ -57,113 +50,26 @@ void display_menu()
|
||||
//
|
||||
void display_config()
|
||||
{
|
||||
hal.console->print("Config: ");
|
||||
hal.console->print(flowSensor.read_register(ADNS3080_CONFIGURATION_BITS),BASE_BIN);
|
||||
hal.scheduler->delay_microseconds(50);
|
||||
hal.console->print(",");
|
||||
hal.console->print(flowSensor.read_register(ADNS3080_EXTENDED_CONFIG),BASE_BIN);
|
||||
hal.scheduler->delay_microseconds(50);
|
||||
hal.console->println();
|
||||
|
||||
// product id
|
||||
hal.console->print("\tproduct id: ");
|
||||
hal.console->print(flowSensor.read_register(ADNS3080_PRODUCT_ID),BASE_HEX);
|
||||
hal.console->printf_P(PSTR("Product id: %#x\n"),optflow.read_register(ADNS3080_PRODUCT_ID));
|
||||
hal.scheduler->delay_microseconds(50);
|
||||
hal.console->print(" (hex)");
|
||||
hal.console->println();
|
||||
|
||||
// frame rate
|
||||
hal.console->print("\tframe rate: ");
|
||||
hal.console->print(flowSensor.get_frame_rate());
|
||||
if( flowSensor.get_frame_rate_auto() == true ) {
|
||||
hal.console->print(" (auto)");
|
||||
}else{
|
||||
hal.console->print(" (manual)");
|
||||
}
|
||||
hal.console->println();
|
||||
|
||||
// resolution
|
||||
hal.console->print("\tresolution: ");
|
||||
hal.console->print(flowSensor.get_resolution());
|
||||
hal.console->println();
|
||||
|
||||
// shutter speed
|
||||
hal.console->print("\tshutter speed: ");
|
||||
hal.console->print(flowSensor.get_shutter_speed());
|
||||
if( flowSensor.get_shutter_speed_auto() ) {
|
||||
hal.console->print(" (auto)");
|
||||
}else{
|
||||
hal.console->print(" (manual)");
|
||||
}
|
||||
hal.console->println();
|
||||
}
|
||||
|
||||
//
|
||||
// set frame rate
|
||||
//
|
||||
void set_frame_rate()
|
||||
{
|
||||
int value;
|
||||
|
||||
// frame rate
|
||||
hal.console->print("frame rate: ");
|
||||
hal.console->print(flowSensor.get_frame_rate());
|
||||
if( flowSensor.get_frame_rate_auto() == true ) {
|
||||
hal.console->print(" (auto)");
|
||||
}else{
|
||||
hal.console->print(" (manual)");
|
||||
}
|
||||
hal.console->println();
|
||||
|
||||
hal.console->println("Choose new frame rate:");
|
||||
hal.console->println("\ta) auto");
|
||||
hal.console->println("\t2) 2000 f/s");
|
||||
hal.console->println("\t3) 3000 f/s");
|
||||
hal.console->println("\t4) 4000 f/s");
|
||||
hal.console->println("\t5) 5000 f/s");
|
||||
hal.console->println("\t6) 6400 f/s");
|
||||
hal.console->println("\tx) exit (leave unchanged)");
|
||||
|
||||
// get user input
|
||||
//hal.console->flush();
|
||||
while( !hal.console->available() ) {
|
||||
hal.scheduler->delay(20);
|
||||
}
|
||||
value = hal.console->read();
|
||||
|
||||
if( value == 'a' || value == 'A')
|
||||
flowSensor.set_frame_rate_auto(true);
|
||||
if( value == '2' )
|
||||
flowSensor.set_frame_rate(2000);
|
||||
if( value == '3' )
|
||||
flowSensor.set_frame_rate(3000);
|
||||
if( value == '4' )
|
||||
flowSensor.set_frame_rate(4000);
|
||||
if( value == '5' )
|
||||
flowSensor.set_frame_rate(5000);
|
||||
if( value == '6' )
|
||||
flowSensor.set_frame_rate(6469);
|
||||
|
||||
// display new frame rate
|
||||
hal.console->print("frame rate: ");
|
||||
hal.console->print(flowSensor.get_frame_rate());
|
||||
if( flowSensor.get_frame_rate_auto() == true ) {
|
||||
hal.console->print(" (auto)");
|
||||
}else{
|
||||
hal.console->print(" (manual)");
|
||||
}
|
||||
hal.console->println();
|
||||
uint8_t reg = optflow.read_register(ADNS3080_CONFIGURATION_BITS);
|
||||
hal.console->printf_P(PSTR("Config registers: %#x"),optflow.read_register(ADNS3080_CONFIGURATION_BITS));
|
||||
hal.scheduler->delay_microseconds(50);
|
||||
hal.console->printf_P(PSTR(",%#x\n"),optflow.read_register(ADNS3080_EXTENDED_CONFIG));
|
||||
hal.scheduler->delay_microseconds(50);
|
||||
}
|
||||
|
||||
// display_image - captures and displays image from flowSensor flowSensor
|
||||
void display_image()
|
||||
{
|
||||
hal.console->println("image data --------------");
|
||||
flowSensor.print_pixel_data();
|
||||
optflow.print_pixel_data();
|
||||
hal.console->println("-------------------------");
|
||||
}
|
||||
|
||||
// display_image - captures and displays image from flowSensor flowSensor
|
||||
// display_image - captures and displays image from flowSensor
|
||||
void display_image_continuously()
|
||||
{
|
||||
int i;
|
||||
@ -179,111 +85,6 @@ void display_image_continuously()
|
||||
i++;
|
||||
}
|
||||
}
|
||||
|
||||
//hal.console->flush();
|
||||
}
|
||||
|
||||
//
|
||||
// set resolutiojn
|
||||
//
|
||||
void set_resolution()
|
||||
{
|
||||
int value;
|
||||
int resolution = flowSensor.get_resolution();
|
||||
hal.console->print("resolution: ");
|
||||
hal.console->println(resolution);
|
||||
hal.console->println("Choose new value:");
|
||||
hal.console->println(" 1) 1600");
|
||||
hal.console->println(" 4) 400");
|
||||
hal.console->println(" x) exit");
|
||||
hal.console->println();
|
||||
|
||||
// get user input
|
||||
//hal.console->flush();
|
||||
while( !hal.console->available() ) {
|
||||
hal.scheduler->delay(20);
|
||||
}
|
||||
value = hal.console->read();
|
||||
|
||||
// update resolution
|
||||
if( value == '1' ) {
|
||||
flowSensor.set_resolution(ADNS3080_RESOLUTION_1600);
|
||||
}
|
||||
if( value == '4' ) {
|
||||
flowSensor.set_resolution(ADNS3080_RESOLUTION_400);
|
||||
}
|
||||
|
||||
hal.console->print("new resolution: ");
|
||||
hal.console->println(flowSensor.get_resolution());
|
||||
}
|
||||
|
||||
//
|
||||
// set shutter speed
|
||||
//
|
||||
void set_shutter_speed()
|
||||
{
|
||||
int value;
|
||||
|
||||
// shutter speed
|
||||
hal.console->print("shutter speed: ");
|
||||
hal.console->print(flowSensor.get_shutter_speed());
|
||||
if( flowSensor.get_shutter_speed_auto() == true ) {
|
||||
hal.console->print(" (auto)");
|
||||
}else{
|
||||
hal.console->print(" (manual)");
|
||||
}
|
||||
hal.console->println();
|
||||
|
||||
hal.console->println("Choose new shutter speed:");
|
||||
hal.console->println("\ta) auto");
|
||||
hal.console->println("\t1) 1000 clock cycles");
|
||||
hal.console->println("\t2) 2000 clock cycles");
|
||||
hal.console->println("\t3) 3000 clock cycles");
|
||||
hal.console->println("\t4) 4000 clock cycles");
|
||||
hal.console->println("\t5) 5000 clock cycles");
|
||||
hal.console->println("\t6) 6000 clock cycles");
|
||||
hal.console->println("\t7) 7000 clock cycles");
|
||||
hal.console->println("\t8) 8000 clock cycles");
|
||||
hal.console->println("\t9) 9000 clock cycles");
|
||||
hal.console->println("\tx) exit (leave unchanged)");
|
||||
|
||||
// get user input
|
||||
//hal.console->flush();
|
||||
while( !hal.console->available() ) {
|
||||
hal.scheduler->delay(20);
|
||||
}
|
||||
value = hal.console->read();
|
||||
|
||||
if( value == 'a' || value == 'A')
|
||||
flowSensor.set_shutter_speed_auto(true);
|
||||
if( value == '1' )
|
||||
flowSensor.set_shutter_speed(1000);
|
||||
if( value == '2' )
|
||||
flowSensor.set_shutter_speed(2000);
|
||||
if( value == '3' )
|
||||
flowSensor.set_shutter_speed(3000);
|
||||
if( value == '4' )
|
||||
flowSensor.set_shutter_speed(4000);
|
||||
if( value == '5' )
|
||||
flowSensor.set_shutter_speed(5000);
|
||||
if( value == '6' )
|
||||
flowSensor.set_shutter_speed(6000);
|
||||
if( value == '7' )
|
||||
flowSensor.set_shutter_speed(7000);
|
||||
if( value == '8' )
|
||||
flowSensor.set_shutter_speed(8000);
|
||||
if( value == '9' )
|
||||
flowSensor.set_shutter_speed(9000);
|
||||
|
||||
// display new shutter speed
|
||||
hal.console->print("shutter speed: ");
|
||||
hal.console->print(flowSensor.get_shutter_speed());
|
||||
if( flowSensor.get_shutter_speed_auto() == true ) {
|
||||
hal.console->print(" (auto)");
|
||||
}else{
|
||||
hal.console->print(" (manual)");
|
||||
}
|
||||
hal.console->println();
|
||||
}
|
||||
|
||||
//
|
||||
@ -292,40 +93,21 @@ void set_shutter_speed()
|
||||
void display_motion()
|
||||
{
|
||||
bool first_time = true;
|
||||
//hal.console->flush();
|
||||
|
||||
// display instructions on how to exit
|
||||
hal.console->println("press x to return to menu..");
|
||||
hal.scheduler->delay(1000);
|
||||
|
||||
while( !hal.console->available() ) {
|
||||
//flowSensor.update();
|
||||
flowSensor.update_position(0,0,0,1,100);
|
||||
|
||||
// check for errors
|
||||
if( flowSensor.overflow() )
|
||||
hal.console->println("overflow!!");
|
||||
while (!hal.console->available()) {
|
||||
optflow.update_position(0,0,0,1,100);
|
||||
|
||||
// x,y,squal
|
||||
hal.console->print("x/dx: ");
|
||||
hal.console->print(flowSensor.x,BASE_DEC);
|
||||
hal.console->print("/");
|
||||
hal.console->print(flowSensor.dx,BASE_DEC);
|
||||
hal.console->print("\ty/dy: ");
|
||||
hal.console->print(flowSensor.y,BASE_DEC);
|
||||
hal.console->print("/");
|
||||
hal.console->print(flowSensor.dy,BASE_DEC);
|
||||
hal.console->print("\tsqual:");
|
||||
hal.console->print(flowSensor.surface_quality,BASE_DEC);
|
||||
hal.console->println();
|
||||
hal.console->printf_P(PSTR("x_cm/dx:%d/%d\ty_cm/dy:%d/%d\tsqual:%d\n"),(int)optflow.x_cm,(int)optflow.dx,(int)optflow.y_cm,(int)optflow.dy,(int)optflow.surface_quality);
|
||||
first_time = false;
|
||||
|
||||
// short delay
|
||||
hal.scheduler->delay(100);
|
||||
}
|
||||
|
||||
// flush the serial
|
||||
//hal.console->flush();
|
||||
}
|
||||
|
||||
void loop()
|
||||
@ -350,11 +132,6 @@ void loop()
|
||||
display_config();
|
||||
break;
|
||||
|
||||
case 'f':
|
||||
// set frame rate
|
||||
set_frame_rate();
|
||||
break;
|
||||
|
||||
case 'i':
|
||||
// display image
|
||||
display_image();
|
||||
@ -370,21 +147,6 @@ void loop()
|
||||
display_motion();
|
||||
break;
|
||||
|
||||
case 'r':
|
||||
// set resolution
|
||||
set_resolution();
|
||||
break;
|
||||
|
||||
case 's':
|
||||
// set shutter speed
|
||||
set_shutter_speed();
|
||||
break;
|
||||
|
||||
case 'z':
|
||||
// clear and reset everything
|
||||
flowSensor.clear_motion();
|
||||
break;
|
||||
|
||||
default:
|
||||
hal.console->println("unrecognised command");
|
||||
hal.console->println();
|
||||
|
||||
Loading…
Reference in New Issue
Block a user