source: flair-src/branches/sanscv/demos/OpticalFlow/uav/src/DemoOpticalFlow.cpp@ 343

//  created:    2011/05/01
//  filename:   DemoOpticalFlow.cpp
//
//  author:     Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    demo optical flow
//
//
/*********************************************************************/

#include "DemoOpticalFlow.h"
#include <Uav.h>
#include <Camera.h>
#include <V4LCamera.h>
#include <CvtColor.h>
#include <OpticalFlow.h>
#include <OpticalFlowCompensated.h>
#include <OpticalFlowSpeed.h>
#include <LowPassFilter.h>
#include <EulerDerivative.h>
#include <Matrix.h>
#include <GridLayout.h>
#include <DataPlot1D.h>
#include <Tab.h>
#include <TabWidget.h>
#include <GroupBox.h>
#include <DoubleSpinBox.h>
#include <PushButton.h>
#include <FrameworkManager.h>
#include <MetaDualShock3.h>
#include <Vector2D.h>
#include <AhrsData.h>
#include <Ahrs.h>
#include <Pid.h>

#include <stdio.h>

using namespace std;
using namespace flair::core;
using namespace flair::gui;
using namespace flair::filter;
using namespace flair::meta;
using namespace flair::sensor;

DemoOpticalFlow::DemoOpticalFlow(TargetController *controller): UavStateMachine(controller) {
    Uav* uav=GetUav();
        if (uav->GetVerticalCamera() == NULL) {
                                Err("no vertical camera found\n");
        exit(1);
    }
        
        startOpticalflow=new PushButton(GetButtonsLayout()->NewRow(),"start optical flow");

        greyCameraImage=new CvtColor(uav->GetVerticalCamera(),"gray",CvtColor::Conversion_t::ToGray);

        uav->GetVerticalCamera()->UseDefaultPlot(greyCameraImage->Output());

        //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");
  opticalFlowCompensated=new OpticalFlowCompensated(opticalFlow,uav->GetAhrs(),uav->GetVerticalCamera()->GetLayout()->NewRow(),"flux optique compense");
  opticalFlowSpeedRaw=new OpticalFlowSpeed(opticalFlowCompensated,uav->GetVerticalCamera()->GetLayout()->NewRow(),"vitesse du Flux Optique");
  //opticalFlowSpeed=vitesse de déplacement en pixels par seconde (moyenne sur tous les points et division par le delta T)
  Matrix* twoByOneOFS=new Matrix((const Thread*)this,2,1,floatType);
  Matrix* twoByOneOFAR=new Matrix((const Thread*)this,2,1,floatType);
  Matrix* twoByOneOFA=new Matrix((const Thread*)this,2,1,floatType);
  opticalFlowSpeed=new LowPassFilter(opticalFlowSpeedRaw,uav->GetVerticalCamera()->GetLayout()->NewRow(),"Speed lowPass",twoByOneOFS);
  opticalFlowAccelerationRaw=new EulerDerivative(opticalFlowSpeed,uav->GetVerticalCamera()->GetLayout()->NewRow(),"derivative",twoByOneOFAR);
  opticalFlowAcceleration=new LowPassFilter(opticalFlowAccelerationRaw,uav->GetVerticalCamera()->GetLayout()->NewRow(),"Acceleration lowPass",twoByOneOFA);
  
  getFrameworkManager()->AddDeviceToLog(opticalFlowSpeedRaw);

  Tab* opticalFlowTab=new Tab(getFrameworkManager()->GetTabWidget(),"flux optique");
  DataPlot1D* xVelocityPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x speed (px/s)",-250,250);
  DataPlot1D* yVelocityPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y speed (px/s)",-250,250);
  
  xVelocityPlot->AddCurve(opticalFlowSpeedRaw->Output()->Element(0,0));
  xVelocityPlot->AddCurve(opticalFlowSpeed->GetMatrix()->Element(0,0),DataPlot::Blue);
  yVelocityPlot->AddCurve(opticalFlowSpeedRaw->Output()->Element(1,0));
  yVelocityPlot->AddCurve(opticalFlowSpeed->GetMatrix()->Element(1,0),DataPlot::Blue);
  
  u_x=new Pid(setupLawTab->At(1,0),"u_x");
  u_x->UseDefaultPlot(graphLawTab->NewRow());
  u_y=new Pid(setupLawTab->At(1,1),"u_y");
  u_y->UseDefaultPlot(graphLawTab->LastRowLastCol());

  opticalFlowGroupBox=new GroupBox(GetTargetController()->GetTab()->NewRow(),"consignes fo");
  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);
  
  Tab* opticalFlowRealTab=new Tab(getFrameworkManager()->GetTabWidget(),"real speed");
  opticalFlowRealSpeed=new Matrix((const Thread*)this,2,1,floatType);
  opticalFlowReference=new Matrix((const Thread*)this,2,1,floatType);
  opticalFlowRealAcceleration=new Matrix((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");
  uav->GetAhrs()->AddPlot(customReferenceOrientation,DataPlot::Yellow);
  AddDataToControlLawLog(customReferenceOrientation);
  
  flagCameraLost=false;
}

void DemoOpticalFlow::SignalEvent(Event_t event) {
    UavStateMachine::SignalEvent(event);
    switch(event) {
    case Event_t::EnteringControlLoop:
        opticalFlowReference->SetValue(0,0,GetTargetController()->GetAxisValue(1)*maxXSpeed->Value());//joy axis 0 maps to x displacement
        opticalFlowReference->SetValue(1,0,GetTargetController()->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;
    }
}

void DemoOpticalFlow::StartOpticalFlow(void) {
        if (SetOrientationMode(OrientationMode_t::Custom)) {
                        Thread::Info("(Re)entering optical flow mode\n");
                        u_x->Reset();
                        u_y->Reset();
        } else {
                        Thread::Warn("Could not enter optical flow mode\n");
        }
}

void DemoOpticalFlow::ExtraCheckPushButton(void) {
        if(startOpticalflow->Clicked()) {
            StartOpticalFlow();
        }
}

void DemoOpticalFlow::ExtraCheckJoystick(void) {
    static bool wasOpticalFlowModeButtonPressed=false;
    // controller button R1 enters optical flow mode
    if(GetTargetController()->IsButtonPressed(9)) { // R1
        if (!wasOpticalFlowModeButtonPressed) {
            wasOpticalFlowModeButtonPressed=true;
            StartOpticalFlow();
        }
    } else {
        wasOpticalFlowModeButtonPressed=false;
    }
}

const AhrsData *DemoOpticalFlow::GetReferenceOrientation(void) {
    Euler refAngles=GetDefaultReferenceOrientation()->GetQuaternion().ToEuler();//to keep default yaw reference

    // /!\ 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
    Vector2Df error, errorVariation; // in Uav coordinate system

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

    u_x->SetValues(error.x, errorVariation.x);
    u_x->Update(GetTime());
    refAngles.pitch=u_x->Output();

    u_y->SetValues(error.y, errorVariation.y);
    u_y->Update(GetTime());
    refAngles.roll=-u_y->Output();

    customReferenceOrientation->SetQuaternionAndAngularRates(refAngles.ToQuaternion(),Vector3Df(0,0,0));

    return customReferenceOrientation;
}

void DemoOpticalFlow::ExtraSecurityCheck(void) {
  if(GetUav()->GetType()=="hds_x8") {
    if(((V4LCamera*)GetUav()->GetVerticalCamera())->HasProblems() && !flagCameraLost) {
      flagCameraLost = true;
      Thread::Err("Camera lost\n");
      SafeStop();
      Land();
    }
  }
}

DemoOpticalFlow::~DemoOpticalFlow() {
}