Changeset 154 in flair-src for trunk/demos/OpticalFlow/uav/src/DemoOpticalFlow.cpp

Timestamp:

03/01/17 17:57:26 (7 years ago)

Author:

Sanahuja Guillaume

Message:

modifs

File:

• trunk/demos/OpticalFlow/uav/src/DemoOpticalFlow.cpp (modified) (3 diffs)

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);