Changeset 154 in flair-src for trunk/demos/OpticalFlow

Timestamp:

Mar 1, 2017, 5:57:26 PM (8 years ago)

Author:

Sanahuja Guillaume

Message:

modifs

Location:

trunk/demos/OpticalFlow/uav

Files:

• build_x86_64/bin/setup_x4.xml (modified) (5 diffs)
• src/DemoOpticalFlow.cpp (modified) (3 diffs)
• src/DemoOpticalFlow.h (modified) (1 diff)

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

@@ -21 +21 @@
      <PushButton value="0" name="reset pitch trim"/>
     </GroupBox>
-   <GroupBox name="consignes fo"><DoubleSpinBox name="debattement x" value="-1.0"/><DoubleSpinBox name="debattement y" value="1.0"/></GroupBox></Tab>
+   <GroupBox name="consignes fo"><DoubleSpinBox name="debattement x" value="-1"/><DoubleSpinBox name="debattement y" value="1"/></GroupBox></Tab>
    <Tab name="Mesures">
     <DataPlot1D name="roll">
@@ -175 +175 @@
       <DoubleSpinBox value="1" name="cutoff frequency"/>
      </GroupBox>
-    <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>
+    <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"/></GroupBox><GroupBox name="Acceleration lowPass"><DoubleSpinBox name="period, 0 for auto" value="0.02"/><DoubleSpinBox name="cutoff frequency" value="2"/></GroupBox></GridLayout>
    </Tab>
    <Tab name="Picture">
@@ -239 +239 @@
     <GroupBox name="u_x">
      <DoubleSpinBox value="0" name="period, 0 for auto"/>
-     <DoubleSpinBox value="0.20" name="kp:"/>
-     <DoubleSpinBox value="0.00" name="ki:"/>
-     <DoubleSpinBox value="1.0" name="sat i:"/>
+     <DoubleSpinBox value="0.2" name="kp:"/>
+     <DoubleSpinBox value="0" name="ki:"/>
+     <DoubleSpinBox value="1" name="sat i:"/>
      <DoubleSpinBox value="0.005" name="kd:"/>
      <DoubleSpinBox value="1" name="sat:"/>
@@ -247 +247 @@
     <GroupBox name="u_y">
      <DoubleSpinBox value="0" name="period, 0 for auto"/>
-     <DoubleSpinBox value="0.20" name="kp:"/>
-     <DoubleSpinBox value="0.00" name="ki:"/>
-     <DoubleSpinBox value="1.0" name="sat i:"/>
+     <DoubleSpinBox value="0.2" name="kp:"/>
+     <DoubleSpinBox value="0" name="ki:"/>
+     <DoubleSpinBox value="1" name="sat i:"/>
      <DoubleSpinBox value="0.005" name="kd:"/>
      <DoubleSpinBox value="1" name="sat:"/>
@@ -284 +284 @@
   </DataPlot1D>
  <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>
+  <Tab name="real speed"><DataPlot1D name="x speed (m/s)" period="100" enabled="1"/><DataPlot1D name="y speed (m/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

@@ -93 +93 @@
         maxXSpeed=new DoubleSpinBox(opticalFlowGroupBox->NewRow(),"debattement x"," m/s",-5,5,0.1,1);
         maxYSpeed=new DoubleSpinBox(opticalFlowGroupBox->LastRowLastCol(),"debattement y"," m/s",-5,5,0.1,1);
-
-        opticalFlowReference=new cvmatrix((const Thread*)this,2,1,floatType);
-        xVelocityPlot->AddCurve(opticalFlowReference->Element(0,0),DataPlot::Green,"consigne");
-        yVelocityPlot->AddCurve(opticalFlowReference->Element(1,0),DataPlot::Green,"consigne");
+  
+  Tab* opticalFlowRealTab=new Tab(getFrameworkManager()->GetTabWidget(),"real speed");
+  opticalFlowRealSpeed=new cvmatrix((const Thread*)this,2,1,floatType);
+  opticalFlowReference=new cvmatrix((const Thread*)this,2,1,floatType);
+  opticalFlowRealAcceleration=new cvmatrix((const Thread*)this,2,1,floatType);
+  DataPlot1D* xRealVelocityPlot=new DataPlot1D(opticalFlowRealTab->NewRow(),"x speed (m/s)",-2,2);
+        DataPlot1D* yRealVelocityPlot=new DataPlot1D(opticalFlowRealTab->LastRowLastCol(),"y speed (m/s)",-2,2);
+  DataPlot1D* xRealAccelerationPlot=new DataPlot1D(opticalFlowRealTab->NewRow(),"x acceleration (m/s2)",-2,2);
+        DataPlot1D* yRealAccelerationPlot=new DataPlot1D(opticalFlowRealTab->LastRowLastCol(),"y acceleration (m/s2)",-2,2);
+  xRealVelocityPlot->AddCurve(opticalFlowRealSpeed->Element(0));
+        xRealVelocityPlot->AddCurve(opticalFlowReference->Element(0),DataPlot::Blue,"consigne");
+        yRealVelocityPlot->AddCurve(opticalFlowRealSpeed->Element(1));
+  yRealVelocityPlot->AddCurve(opticalFlowReference->Element(1),DataPlot::Blue,"consigne");
+  xRealAccelerationPlot->AddCurve(opticalFlowRealAcceleration->Element(0));
+  yRealAccelerationPlot->AddCurve(opticalFlowRealAcceleration->Element(1));
 
         customReferenceOrientation= new AhrsData(this,"reference");
@@ -108 +119 @@
         opticalFlowReference->SetValue(0,0,GetJoystick()->GetAxisValue(1)*maxXSpeed->Value());//joy axis 0 maps to x displacement
         opticalFlowReference->SetValue(1,0,GetJoystick()->GetAxisValue(0)*maxYSpeed->Value());//joy axis 1 maps to y displacement
+        float focal=271.76;
+        float z,dz;
+        AltitudeValues(z, dz);
+        float scale=z/focal;
+        opticalFlowRealSpeed->SetValue(0,0,opticalFlowSpeed->Output(0,0)*scale);
+        opticalFlowRealSpeed->SetValue(1,0,opticalFlowSpeed->Output(1,0)*scale);
+        opticalFlowRealAcceleration->SetValue(0,0,opticalFlowAcceleration->Output(0,0)*scale);
+        opticalFlowRealAcceleration->SetValue(1,0,opticalFlowAcceleration->Output(1,0)*scale);
         break;
     }
@@ -146 +165 @@
     // /!\ 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
-    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;
+
+    error.x=opticalFlowRealSpeed->Value(0,0)-opticalFlowReference->Value(0,0);
+    error.y=opticalFlowRealSpeed->Value(1,0)-opticalFlowReference->Value(1,0);
+    errorVariation.x=opticalFlowRealAcceleration->Value(0,0);
+    errorVariation.y=opticalFlowRealAcceleration->Value(1,0);
 //Printf("Altitude=%f, Error=(%f,%f)\n",z,error.x,error.y);
 

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

@@ -66 +66 @@
                                 flair::gui::PushButton *startOpticalflow,*stopOpticalflow;
                                 void StartOpticalFlow(void);
+        flair::core::cvmatrix *opticalFlowRealSpeed,*opticalFlowRealAcceleration;
 };