Copter: integrate shrunken optflow

ArduCopter/ArduCopter.pde

@@ -341,8 +341,6 @@ static SITL sitl;
 ////////////////////////////////////////////////////////////////////////////////
  #if OPTFLOW == ENABLED
 static AP_OpticalFlow_ADNS3080 optflow;
- #else
-static AP_OpticalFlow optflow;
  #endif
 
 ////////////////////////////////////////////////////////////////////////////////

ArduCopter/Log.pde

@@ -239,8 +239,6 @@ struct PACKED log_Optflow {
     uint8_t surface_quality;
     int16_t x_cm;
     int16_t y_cm;
-    float   latitude;
-    float   longitude;
     int32_t roll;
     int32_t pitch;
 };
@@ -256,8 +254,6 @@ static void Log_Write_Optflow()
         surface_quality : optflow.surface_quality,
         x_cm            : (int16_t) optflow.x_cm,
         y_cm            : (int16_t) optflow.y_cm,
-        latitude        : optflow.vlat,
-        longitude       : optflow.vlon,
         roll            : of_roll,
         pitch           : of_pitch
     };
@@ -737,7 +733,7 @@ static const struct LogStructure log_structure[] PROGMEM = {
     { LOG_CURRENT_MSG, sizeof(log_Current),             
       "CURR", "hIhhhf",      "ThrOut,ThrInt,Volt,Curr,Vcc,CurrTot" },
     { LOG_OPTFLOW_MSG, sizeof(log_Optflow),       
-      "OF",   "hhBccffee",   "Dx,Dy,SQual,X,Y,Lat,Lng,Roll,Pitch" },
+      "OF",   "hhBccee",   "Dx,Dy,SQual,X,Y,Roll,Pitch" },
     { LOG_NAV_TUNING_MSG, sizeof(log_Nav_Tuning),       
       "NTUN", "Ecffffffffee",    "WPDst,WPBrg,PErX,PErY,DVelX,DVelY,VelX,VelY,DAcX,DAcY,DRol,DPit" },
     { LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning),     

ArduCopter/config.h

@@ -369,15 +369,6 @@
 #ifndef OPTFLOW                         // sets global enabled/disabled flag for optflow (as seen in CLI)
  # define OPTFLOW                       ENABLED
 #endif
-#ifndef OPTFLOW_ORIENTATION
- # define OPTFLOW_ORIENTATION    AP_OPTICALFLOW_ADNS3080_PINS_FORWARD
-#endif
-#ifndef OPTFLOW_RESOLUTION
- # define OPTFLOW_RESOLUTION     ADNS3080_RESOLUTION_1600
-#endif
-#ifndef OPTFLOW_FOV
- # define OPTFLOW_FOV                    AP_OPTICALFLOW_ADNS3080_08_FOV
-#endif
 // optical flow based loiter PI values
 #ifndef OPTFLOW_ROLL_P
  #define OPTFLOW_ROLL_P 2.5f

ArduCopter/sensors.pde

@@ -78,21 +78,11 @@ static void init_compass()
 static void init_optflow()
 {
 #if OPTFLOW == ENABLED
-    if( optflow.init() == false ) {
+    optflow.init();
+    if (!optflow.healthy()) {
         g.optflow_enabled = false;
-        cliSerial->print_P(PSTR("\nFailed to Init OptFlow "));
+        cliSerial->print_P(PSTR("Failed to Init OptFlow\n"));
         Log_Write_Error(ERROR_SUBSYSTEM_OPTFLOW,ERROR_CODE_FAILED_TO_INITIALISE);
-    }else{
-        // suspend timer while we set-up SPI communication
-        hal.scheduler->suspend_timer_procs();
-
-        optflow.set_orientation(OPTFLOW_ORIENTATION);   // set optical flow sensor's orientation on aircraft
-        optflow.set_frame_rate(2000);                   // set minimum update rate (which should lead to maximum low light performance
-        optflow.set_resolution(OPTFLOW_RESOLUTION);     // set optical flow sensor's resolution
-        optflow.set_field_of_view(OPTFLOW_FOV);         // set optical flow sensor's field of view
-
-        // resume timer
-        hal.scheduler->resume_timer_procs();
     }
 #endif      // OPTFLOW == ENABLED
 }

ArduCopter/test.pde

@@ -286,10 +286,8 @@ test_optflow(uint8_t argc, const Menu::arg *argv)
         while(1) {
             delay(200);
             optflow.update();
-            cliSerial->printf_P(PSTR("x/dx: %d/%d\t y/dy %d/%d\t squal:%d\n"),
-                            optflow.x,
+            cliSerial->printf_P(PSTR("dx:%d\t dy:%d\t squal:%d\n"),
                             optflow.dx,
-                            optflow.y,
                             optflow.dy,
                             optflow.surface_quality);