Changeset 122 in flair-src for trunk/demos/CircleFollower/uav


Ignore:
Timestamp:
Jan 6, 2017, 1:56:26 PM (5 years ago)
Author:
Sanahuja Guillaume
Message:

modifs uav vrpn i686

Location:
trunk/demos/CircleFollower/uav
Files:
1 deleted
3 edited

Legend:

Unmodified
Added
Removed
  • trunk/demos/CircleFollower/uav/src/CircleFollower.cpp

    r121 r122  
    3939using namespace flair::meta;
    4040
    41 CircleFollower::CircleFollower(Uav* uav,TargetController *controller): UavStateMachine(uav,controller), behaviourMode(BehaviourMode_t::Default), vrpnLost(false) {
    42     uav->SetupVRPNAutoIP(uav->ObjectName());
    43 
     41CircleFollower::CircleFollower(TargetController *controller): UavStateMachine(controller), behaviourMode(BehaviourMode_t::Default), vrpnLost(false) {
     42    Uav* uav=GetUav();
     43               
     44                VrpnClient* vrpnclient=new VrpnClient("vrpn", uav->GetDefaultVrpnAddress(),10000, 80);
     45                uavVrpn = new MetaVrpnObject(uav->ObjectName());
     46                getFrameworkManager()->AddDeviceToLog(uavVrpn);
     47                uav->GetAhrs()->YawPlot()->AddCurve(uavVrpn->State()->Element(2),DataPlot::Green);
     48                                                                                                                                 
    4449    startCircle=new PushButton(GetButtonsLayout()->NewRow(),"start_circle");
    4550    stopCircle=new PushButton(GetButtonsLayout()->LastRowLastCol(),"stop_circle");
    4651
    47     if(uav->GetVrpnClient()->UseXbee()==true) {
    48         targetVrpn=new MetaVrpnObject(uav->GetVrpnClient(),"target",1);
     52    if(vrpnclient->UseXbee()==true) {
     53        targetVrpn=new MetaVrpnObject("target",1);
    4954    } else {
    50         targetVrpn=new MetaVrpnObject(uav->GetVrpnClient(),"target");
     55        targetVrpn=new MetaVrpnObject("target");
    5156    }
    5257
    5358    getFrameworkManager()->AddDeviceToLog(targetVrpn);
    5459
    55     circle=new TrajectoryGenerator2DCircle(uav->GetVrpnClient()->GetLayout()->NewRow(),"circle");
    56     uav->GetVrpnObject()->xPlot()->AddCurve(circle->Matrix()->Element(0,0),DataPlot::Blue);
    57     uav->GetVrpnObject()->yPlot()->AddCurve(circle->Matrix()->Element(0,1),DataPlot::Blue);
    58     uav->GetVrpnObject()->VxPlot()->AddCurve(circle->Matrix()->Element(1,0),DataPlot::Blue);
    59     uav->GetVrpnObject()->VyPlot()->AddCurve(circle->Matrix()->Element(1,1),DataPlot::Blue);
    60     uav->GetVrpnObject()->XyPlot()->AddCurve(circle->Matrix()->Element(0,1),circle->Matrix()->Element(0,0),DataPlot::Blue,"circle");
     60    circle=new TrajectoryGenerator2DCircle(vrpnclient->GetLayout()->NewRow(),"circle");
     61    uavVrpn->xPlot()->AddCurve(circle->Matrix()->Element(0,0),DataPlot::Blue);
     62    uavVrpn->yPlot()->AddCurve(circle->Matrix()->Element(0,1),DataPlot::Blue);
     63    uavVrpn->VxPlot()->AddCurve(circle->Matrix()->Element(1,0),DataPlot::Blue);
     64    uavVrpn->VyPlot()->AddCurve(circle->Matrix()->Element(1,1),DataPlot::Blue);
     65    uavVrpn->XyPlot()->AddCurve(circle->Matrix()->Element(0,1),circle->Matrix()->Element(0,0),DataPlot::Blue,"circle");
    6166
    6267    uX=new Pid(setupLawTab->At(1,0),"u_x");
     
    7984    //get yaw from vrpn
    8085    Euler vrpnEuler;
    81     GetUav()->GetVrpnObject()->GetEuler(vrpnEuler);
     86    uavVrpn->GetEuler(vrpnEuler);
    8287
    8388    //get roll, pitch and w from imu
     
    98103    Vector3D uav_pos,uav_vel;
    99104
    100     GetUav()->GetVrpnObject()->GetPosition(uav_pos);
    101     GetUav()->GetVrpnObject()->GetSpeed(uav_vel);
     105    uavVrpn->GetPosition(uav_pos);
     106    uavVrpn->GetSpeed(uav_vel);
    102107    //z and dz must be in uav's frame
    103108    z=-uav_pos.z;
     
    131136    Vector2D uav_2Dpos,uav_2Dvel; // in VRPN coordinate system
    132137
    133     GetUav()->GetVrpnObject()->GetPosition(uav_pos);
    134     GetUav()->GetVrpnObject()->GetSpeed(uav_vel);
     138    uavVrpn->GetPosition(uav_pos);
     139    uavVrpn->GetSpeed(uav_vel);
    135140
    136141    uav_pos.To2Dxy(uav_2Dpos);
     
    195200            Land();
    196201        }
    197         if (!GetUav()->GetVrpnObject()->IsTracked(500)) {
     202        if (!uavVrpn->IsTracked(500)) {
    198203            Thread::Err("VRPN, uav lost\n");
    199204            vrpnLost=true;
     
    239244    circle->SetCenter(target_2Dpos);
    240245
    241     GetUav()->GetVrpnObject()->GetPosition(uav_pos);
     246    uavVrpn->GetPosition(uav_pos);
    242247    uav_pos.To2Dxy(uav_2Dpos);
    243248    circle->StartTraj(uav_2Dpos);
     
    258263    Vector3D vrpn_pos;
    259264
    260     GetUav()->GetVrpnObject()->GetEuler(vrpn_euler);
     265    uavVrpn->GetEuler(vrpn_euler);
    261266    yawHold=vrpn_euler.yaw;
    262267
    263     GetUav()->GetVrpnObject()->GetPosition(vrpn_pos);
     268    uavVrpn->GetPosition(vrpn_pos);
    264269    vrpn_pos.To2Dxy(posHold);
    265270
  • trunk/demos/CircleFollower/uav/src/CircleFollower.h

    r38 r122  
    3434class CircleFollower : public flair::meta::UavStateMachine {
    3535    public:
    36         CircleFollower(flair::meta::Uav* uav,flair::sensor::TargetController *controller);
     36        CircleFollower(flair::sensor::TargetController *controller);
    3737        ~CircleFollower();
    3838
     
    6565
    6666        flair::gui::PushButton *startCircle,*stopCircle;
    67         flair::meta::MetaVrpnObject *targetVrpn;
     67        flair::meta::MetaVrpnObject *targetVrpn,*uavVrpn;
    6868        flair::filter::TrajectoryGenerator2DCircle *circle;
    6969        flair::core::AhrsData *customReferenceOrientation,*customOrientation;
  • trunk/demos/CircleFollower/uav/src/main.cpp

    r38 r122  
    4545    manager->SetupLogger(log_path);
    4646
    47     Uav* drone=CreateUav(manager,name,uav_type);
     47    Uav* drone=CreateUav(name,uav_type);
    4848    TargetEthController *controller=new TargetEthController(manager,"Dualshock3",ds3port);
    49     CircleFollower* demo=new CircleFollower(drone,controller);
     49    CircleFollower* demo=new CircleFollower(controller);
    5050
    5151    demo->Start();
Note: See TracChangeset for help on using the changeset viewer.