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


Ignore:
Timestamp:
03/01/17 17:57:26 (5 years ago)
Author:
Sanahuja Guillaume
Message:

modifs

Location:
trunk/demos/OpticalFlow/uav/src
Files:
2 edited

Legend:

Unmodified
Added
Removed
  • trunk/demos/OpticalFlow/uav/src/DemoOpticalFlow.cpp

    r147 r154  
    9393        maxXSpeed=new DoubleSpinBox(opticalFlowGroupBox->NewRow(),"debattement x"," m/s",-5,5,0.1,1);
    9494        maxYSpeed=new DoubleSpinBox(opticalFlowGroupBox->LastRowLastCol(),"debattement y"," m/s",-5,5,0.1,1);
    95 
    96         opticalFlowReference=new cvmatrix((const Thread*)this,2,1,floatType);
    97         xVelocityPlot->AddCurve(opticalFlowReference->Element(0,0),DataPlot::Green,"consigne");
    98         yVelocityPlot->AddCurve(opticalFlowReference->Element(1,0),DataPlot::Green,"consigne");
     95 
     96  Tab* opticalFlowRealTab=new Tab(getFrameworkManager()->GetTabWidget(),"real speed");
     97  opticalFlowRealSpeed=new cvmatrix((const Thread*)this,2,1,floatType);
     98  opticalFlowReference=new cvmatrix((const Thread*)this,2,1,floatType);
     99  opticalFlowRealAcceleration=new cvmatrix((const Thread*)this,2,1,floatType);
     100  DataPlot1D* xRealVelocityPlot=new DataPlot1D(opticalFlowRealTab->NewRow(),"x speed (m/s)",-2,2);
     101        DataPlot1D* yRealVelocityPlot=new DataPlot1D(opticalFlowRealTab->LastRowLastCol(),"y speed (m/s)",-2,2);
     102  DataPlot1D* xRealAccelerationPlot=new DataPlot1D(opticalFlowRealTab->NewRow(),"x acceleration (m/s2)",-2,2);
     103        DataPlot1D* yRealAccelerationPlot=new DataPlot1D(opticalFlowRealTab->LastRowLastCol(),"y acceleration (m/s2)",-2,2);
     104  xRealVelocityPlot->AddCurve(opticalFlowRealSpeed->Element(0));
     105        xRealVelocityPlot->AddCurve(opticalFlowReference->Element(0),DataPlot::Blue,"consigne");
     106        yRealVelocityPlot->AddCurve(opticalFlowRealSpeed->Element(1));
     107  yRealVelocityPlot->AddCurve(opticalFlowReference->Element(1),DataPlot::Blue,"consigne");
     108  xRealAccelerationPlot->AddCurve(opticalFlowRealAcceleration->Element(0));
     109  yRealAccelerationPlot->AddCurve(opticalFlowRealAcceleration->Element(1));
    99110
    100111        customReferenceOrientation= new AhrsData(this,"reference");
     
    108119        opticalFlowReference->SetValue(0,0,GetJoystick()->GetAxisValue(1)*maxXSpeed->Value());//joy axis 0 maps to x displacement
    109120        opticalFlowReference->SetValue(1,0,GetJoystick()->GetAxisValue(0)*maxYSpeed->Value());//joy axis 1 maps to y displacement
     121        float focal=271.76;
     122        float z,dz;
     123        AltitudeValues(z, dz);
     124        float scale=z/focal;
     125        opticalFlowRealSpeed->SetValue(0,0,opticalFlowSpeed->Output(0,0)*scale);
     126        opticalFlowRealSpeed->SetValue(1,0,opticalFlowSpeed->Output(1,0)*scale);
     127        opticalFlowRealAcceleration->SetValue(0,0,opticalFlowAcceleration->Output(0,0)*scale);
     128        opticalFlowRealAcceleration->SetValue(1,0,opticalFlowAcceleration->Output(1,0)*scale);
    110129        break;
    111130    }
     
    146165    // /!\ 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
    147166    Vector2D error, errorVariation; // in Uav coordinate system
    148     float focal=271.76;
    149     float z,dz;
    150     AltitudeValues(z, dz);
    151     float scale=z/focal;
    152     error.x=opticalFlowSpeed->Output(0,0)*scale-opticalFlowReference->Value(0,0);
    153     error.y=opticalFlowSpeed->Output(1,0)*scale-opticalFlowReference->Value(1,0);
    154     errorVariation.x=opticalFlowAcceleration->Output(0,0)*scale;
    155     errorVariation.y=opticalFlowAcceleration->Output(1,0)*scale;
     167
     168    error.x=opticalFlowRealSpeed->Value(0,0)-opticalFlowReference->Value(0,0);
     169    error.y=opticalFlowRealSpeed->Value(1,0)-opticalFlowReference->Value(1,0);
     170    errorVariation.x=opticalFlowRealAcceleration->Value(0,0);
     171    errorVariation.y=opticalFlowRealAcceleration->Value(1,0);
    156172//Printf("Altitude=%f, Error=(%f,%f)\n",z,error.x,error.y);
    157173
  • trunk/demos/OpticalFlow/uav/src/DemoOpticalFlow.h

    r142 r154  
    6666                                flair::gui::PushButton *startOpticalflow,*stopOpticalflow;
    6767                                void StartOpticalFlow(void);
     68        flair::core::cvmatrix *opticalFlowRealSpeed,*opticalFlowRealAcceleration;
    6869};
    6970
Note: See TracChangeset for help on using the changeset viewer.