Changeset 143 in flair-src for trunk/demos/OpticalFlow/uav

Timestamp:

Feb 21, 2017, 4:37:58 PM (8 years ago)

Author:

Bayard Gildas

Message:

Plop

Location:

trunk/demos/OpticalFlow/uav

Files:

• build_x86_64/bin/setup_x4.xml (modified) (7 diffs)
• src/DemoOpticalFlow.cpp (modified) (3 diffs)

trunk/demos/OpticalFlow/uav/build_x86_64/bin/setup_x4.xml

@@ -11 +11 @@
     </GroupBox>
     <GroupBox name="consignes joy">
-     <DoubleSpinBox value="-20" name="debattement roll"/>
-     <DoubleSpinBox value="-20" name="debattement pitch"/>
+     <DoubleSpinBox value="-10" name="debattement roll"/>
+     <DoubleSpinBox value="-10" name="debattement pitch"/>
      <DoubleSpinBox value="90" name="debattement wz"/>
      <DoubleSpinBox value="-0.7" name="debattement dz"/>
@@ -21 +21 @@
      <PushButton value="0" name="reset pitch trim"/>
     </GroupBox>
-   <GroupBox name="consignes fo"><DoubleSpinBox name="debattement x" value="-1"/><DoubleSpinBox name="debattement y" value="1"/></GroupBox></Tab>
+   <GroupBox name="consignes fo"><DoubleSpinBox name="debattement x" value="-1.0"/><DoubleSpinBox name="debattement y" value="1.0"/></GroupBox></Tab>
    <Tab name="Mesures">
     <DataPlot1D name="roll">
@@ -153 +153 @@
     </DataPlot1D>
    </Tab>
-  </TabWidget>
+  <Tab name="Quaternion"><DataPlot1D name="q0" period="100" enabled="0"/><DataPlot1D name="q1" period="100" enabled="0"/><DataPlot1D name="q2" period="100" enabled="0"/><DataPlot1D name="q3" period="100" enabled="0"/></Tab><Tab name="IMU"><DataPlot1D name="acc_x" period="100" enabled="0"/><DataPlot1D name="acc_y" period="100" enabled="0"/><DataPlot1D name="acc_z" period="100" enabled="0"/><DataPlot1D name="gyr_x" period="100" enabled="0"/><DataPlot1D name="gyr_y" period="100" enabled="0"/><DataPlot1D name="gyr_z" period="100" enabled="0"/><DataPlot1D name="mag_x" period="100" enabled="0"/><DataPlot1D name="mag_y" period="100" enabled="0"/><DataPlot1D name="mag_z" period="100" enabled="0"/></Tab></TabWidget>
  </Tab>
  <Tab name="battery">
@@ -175 +175 @@
       <DoubleSpinBox value="1" name="cutoff frequency"/>
      </GroupBox>
-    </GridLayout>
+    <GroupBox name="derivative"><DoubleSpinBox name="period, 0 for auto:" value="0.02"/></GroupBox><GroupBox name="Speed lowPass"><DoubleSpinBox name="period, 0 for auto" value="0.02"/><DoubleSpinBox name="cutoff frequency" value="2.00"/></GroupBox><GroupBox name="Acceleration lowPass"><DoubleSpinBox name="period, 0 for auto" value="0.02"/><DoubleSpinBox name="cutoff frequency" value="2.00"/></GroupBox></GridLayout>
    </Tab>
    <Tab name="Picture">
-    <Picture name="simu_cam_v">
+    <Picture name="simu_cam_v" period="200" enabled="0">
      <Setup period="50" enabled="1"/>
     </Picture>
@@ -191 +191 @@
    <PushButton value="1" name="take_off"/>
    <PushButton value="0" name="land"/>
-  </GridLayout>
+  <PushButton name="start optical flow"/></GridLayout>
   <GroupBox name="takeoff/landing">
    <DoubleSpinBox value="1" name="desired takeoff altitude"/>
@@ -239 +239 @@
     <GroupBox name="u_x">
      <DoubleSpinBox value="0" name="period, 0 for auto"/>
-     <DoubleSpinBox value="0.030" name="kp:"/>
-     <DoubleSpinBox value="0.020" name="ki:"/>
-     <DoubleSpinBox value="0.20" name="sat i:"/>
-     <DoubleSpinBox value="0.1" name="kd:"/>
+     <DoubleSpinBox value="0.20" name="kp:"/>
+     <DoubleSpinBox value="0.00" name="ki:"/>
+     <DoubleSpinBox value="1.0" name="sat i:"/>
+     <DoubleSpinBox value="0.005" name="kd:"/>
      <DoubleSpinBox value="1" name="sat:"/>
     </GroupBox>
     <GroupBox name="u_y">
      <DoubleSpinBox value="0" name="period, 0 for auto"/>
-     <DoubleSpinBox value="0.03" name="kp:"/>
-     <DoubleSpinBox value="0.020" name="ki:"/>
-     <DoubleSpinBox value="0.20" name="sat i:"/>
-     <DoubleSpinBox value="0.2" name="kd:"/>
+     <DoubleSpinBox value="0.20" name="kp:"/>
+     <DoubleSpinBox value="0.00" name="ki:"/>
+     <DoubleSpinBox value="1.0" name="sat i:"/>
+     <DoubleSpinBox value="0.005" name="kd:"/>
      <DoubleSpinBox value="1" name="sat:"/>
     </GroupBox>
@@ -283 +283 @@
    <Setup period="100" enabled="1"/>
   </DataPlot1D>
- </Tab>
-  </TabWidget>
-<TabWidget name="App_TabWidget"><Tab name="x4_0"><PushButton name="save config on target" value="1"/></Tab></TabWidget></root>
+ <DataPlot1D name="x_velocity_derivative" period="100" enabled="1"/><DataPlot1D name="y_velocity_derivative" period="100" enabled="1"/><DataPlot1D name="x_acceleration" period="100" enabled="1"/><DataPlot1D name="y_acceleration" period="100" enabled="1"/><DataPlot1D name="x speed (px/s)" period="100" enabled="1"/><DataPlot1D name="y speed (px/s)" period="100" enabled="1"/></Tab>
+  </TabWidget>
+<TabWidget name="App_TabWidget"><Tab name="x4_0"><PushButton name="save config on target" value="1"/></Tab></TabWidget><PushButton name="save config on target (x4_0)" value="1"/></root>

trunk/demos/OpticalFlow/uav/src/DemoOpticalFlow.cpp

@@ -58 +58 @@
 
         //optical flow stack
+        //opticalFlow= matrice de déplacements en pixels entre 2 images consécutives
         opticalFlow=new OpticalFlow(greyCameraImage,uav->GetVerticalCamera()->GetLayout()->NewRow(),"flux optique");
         opticalFlowSpeedRaw=new OpticalFlowSpeed(opticalFlow,"vitesse du flux optique");
+        //opticalFlowSpeed=vitesse de déplacement en pixel par seconde (moyenne sur tous les points et division par le delta T)
         cvmatrix* twoByOneSpeed=new cvmatrix((const Thread*)this,2,1,floatType);
         opticalFlowSpeed=new LowPassFilter(opticalFlowSpeedRaw,uav->GetVerticalCamera()->GetLayout()->NewRow(),"Speed lowPass",twoByOneSpeed);
@@ -70 +72 @@
 
         Tab* opticalFlowTab=new Tab(getFrameworkManager()->GetTabWidget(),"flux optique");
-        DataPlot1D* xVelocityPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x_velocity",-5,5);
-        DataPlot1D* yVelocityPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y_velocity",-5,5);
-        DataPlot1D* xAccelerationPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x_acceleration",-5,5);
-        DataPlot1D* yAccelerationPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y_acceleration",-5,5);
+        DataPlot1D* xVelocityPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x speed (px/s)",-250,250);
+        DataPlot1D* yVelocityPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y speed (px/s)",-250,250);
+        DataPlot1D* xAccelerationPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x_acceleration",-250,250);
+        DataPlot1D* yAccelerationPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y_acceleration",-250,250);
 
         xVelocityPlot->AddCurve(opticalFlowSpeedRaw->Output()->Element(0,0));
@@ -143 +145 @@
     Euler refAngles=GetDefaultReferenceOrientation()->GetQuaternion().ToEuler();//to keep default yaw reference
 
-    // /!\ in this demo, the target value is a speed (in pixel/s). As a consequence the error is the difference between the current speed and the target speed
+    // /!\ in this demo, the target value is a speed (in m/s). As a consequence the error is the difference between the current speed and the target speed
     Vector2D error, errorVariation; // in Uav coordinate system
-
-    //opticalFlow= matrice de déplacements en pixels entre 2 images consécutives
-    //opticalFlowSpeed=vitesse de déplacement en pixel par seconde (moyenne sur tous les points et division par le delta T)
-    error.x=opticalFlowSpeed->Output(0,0)-opticalFlowReference->Value(0,0);
-    error.y=opticalFlowSpeed->Output(1,0)-opticalFlowReference->Value(1,0);
-    errorVariation.x=opticalFlowAcceleration->Output(0,0);
-    errorVariation.y=opticalFlowAcceleration->Output(1,0);
+    float focal=271.76;
+    float z,dz;
+    AltitudeValues(z, dz);
+    float scale=z/focal;
+    error.x=opticalFlowSpeed->Output(0,0)*scale-opticalFlowReference->Value(0,0);
+    error.y=opticalFlowSpeed->Output(1,0)*scale-opticalFlowReference->Value(1,0);
+    errorVariation.x=opticalFlowAcceleration->Output(0,0)*scale;
+    errorVariation.y=opticalFlowAcceleration->Output(1,0)*scale;
+//Printf("Altitude=%f, Error=(%f,%f)\n",z,error.x,error.y);
 
     u_x->SetValues(error.x, errorVariation.x);