Index: /trunk/demos/OpticalFlow/uav/build_x86_64/bin/setup_x4.xml
===================================================================
--- /trunk/demos/OpticalFlow/uav/build_x86_64/bin/setup_x4.xml (revision 153)
+++ /trunk/demos/OpticalFlow/uav/build_x86_64/bin/setup_x4.xml (revision 154)
@@ -21,5 +21,5 @@
-
+
@@ -175,5 +175,5 @@
-
+
@@ -239,7 +239,7 @@
-
-
-
+
+
+
@@ -247,7 +247,7 @@
-
-
-
+
+
+
@@ -284,4 +284,4 @@
-
+
Index: /trunk/demos/OpticalFlow/uav/src/DemoOpticalFlow.cpp
===================================================================
--- /trunk/demos/OpticalFlow/uav/src/DemoOpticalFlow.cpp (revision 153)
+++ /trunk/demos/OpticalFlow/uav/src/DemoOpticalFlow.cpp (revision 154)
@@ -93,8 +93,19 @@
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,4 +119,12 @@
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,12 +165,9 @@
// /!\ 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);
Index: /trunk/demos/OpticalFlow/uav/src/DemoOpticalFlow.h
===================================================================
--- /trunk/demos/OpticalFlow/uav/src/DemoOpticalFlow.h (revision 153)
+++ /trunk/demos/OpticalFlow/uav/src/DemoOpticalFlow.h (revision 154)
@@ -66,4 +66,5 @@
flair::gui::PushButton *startOpticalflow,*stopOpticalflow;
void StartOpticalFlow(void);
+ flair::core::cvmatrix *opticalFlowRealSpeed,*opticalFlowRealAcceleration;
};