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

modifs uav vrpn i686

File:
1 edited

Legend:

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

    r104 r122  
    5050
    5151
    52 SimpleFleet::SimpleFleet(flair::meta::Uav* uav,string broadcast,TargetController *controller): UavStateMachine(uav,controller), behaviourMode(BehaviourMode_t::Default), vrpnLost(false) {
    53     uav->SetupVRPNAutoIP(uav->ObjectName());
    54 
    55     circle=new TrajectoryGenerator2DCircle(uav->GetVrpnClient()->GetLayout()->NewRow(),"circle");
    56     uav->GetVrpnObject()->xPlot()->AddCurve(circle->Matrix()->Element(0,0),0,0,255);
    57     uav->GetVrpnObject()->yPlot()->AddCurve(circle->Matrix()->Element(0,1),0,0,255);
    58     uav->GetVrpnObject()->VxPlot()->AddCurve(circle->Matrix()->Element(1,0),0,0,255);
    59     uav->GetVrpnObject()->VyPlot()->AddCurve(circle->Matrix()->Element(1,1),0,0,255);
    60 
    61     xCircleCenter=new DoubleSpinBox(uav->GetVrpnClient()->GetLayout()->NewRow(),"x circle center"," m",-5,5,0.1,1,0);
    62     yCircleCenter=new DoubleSpinBox(uav->GetVrpnClient()->GetLayout()->NewRow(),"y circle center"," m",-5,5,0.1,1,0);
    63     yDisplacement=new DoubleSpinBox(uav->GetVrpnClient()->GetLayout()->NewRow(),"y displacement"," m",0,2,0.1,1,0);
     52SimpleFleet::SimpleFleet(string broadcast,TargetController *controller): UavStateMachine(controller), behaviourMode(BehaviourMode_t::Default), vrpnLost(false) {
     53    Uav* uav=GetUav();
     54               
     55                VrpnClient* vrpnclient=new VrpnClient("vrpn", uav->GetDefaultVrpnAddress(),10000, 80);
     56                uavVrpn = new MetaVrpnObject(uav->ObjectName());
     57                getFrameworkManager()->AddDeviceToLog(uavVrpn);
     58                uav->GetAhrs()->YawPlot()->AddCurve(uavVrpn->State()->Element(2),DataPlot::Green);
     59
     60    circle=new TrajectoryGenerator2DCircle(vrpnclient->GetLayout()->NewRow(),"circle");
     61    uavVrpn->xPlot()->AddCurve(circle->Matrix()->Element(0,0),0,0,255);
     62    uavVrpn->yPlot()->AddCurve(circle->Matrix()->Element(0,1),0,0,255);
     63    uavVrpn->VxPlot()->AddCurve(circle->Matrix()->Element(1,0),0,0,255);
     64    uavVrpn->VyPlot()->AddCurve(circle->Matrix()->Element(1,1),0,0,255);
     65
     66    xCircleCenter=new DoubleSpinBox(vrpnclient->GetLayout()->NewRow(),"x circle center"," m",-5,5,0.1,1,0);
     67    yCircleCenter=new DoubleSpinBox(vrpnclient->GetLayout()->NewRow(),"y circle center"," m",-5,5,0.1,1,0);
     68    yDisplacement=new DoubleSpinBox(vrpnclient->GetLayout()->NewRow(),"y displacement"," m",0,2,0.1,1,0);
    6469
    6570    //parent->AddDeviceToLog(Uz());
     
    101106    //get yaw from vrpn
    102107    Euler vrpnEuler;
    103     GetUav()->GetVrpnObject()->GetEuler(vrpnEuler);
     108    uavVrpn->GetEuler(vrpnEuler);
    104109
    105110    //get roll, pitch and w from imu
     
    120125    Vector3D uav_pos,uav_vel;
    121126
    122     GetUav()->GetVrpnObject()->GetPosition(uav_pos);
    123     GetUav()->GetVrpnObject()->GetSpeed(uav_vel);
     127    uavVrpn->GetPosition(uav_pos);
     128    uavVrpn->GetSpeed(uav_vel);
    124129    //z and dz must be in uav's frame
    125130    z=-uav_pos.z;
     
    154159    Euler vrpn_euler; // in VRPN coordinate system
    155160
    156     GetUav()->GetVrpnObject()->GetPosition(uav_pos);
    157     GetUav()->GetVrpnObject()->GetSpeed(uav_vel);
    158     GetUav()->GetVrpnObject()->GetEuler(vrpn_euler);
     161    uavVrpn->GetPosition(uav_pos);
     162    uavVrpn->GetSpeed(uav_vel);
     163    uavVrpn->GetEuler(vrpn_euler);
    159164
    160165    uav_pos.To2Dxy(uav_2Dpos);
     
    294299void SimpleFleet::ExtraSecurityCheck(void) {
    295300    if (!vrpnLost && behaviourMode!=BehaviourMode_t::Default) {
    296         if (!GetUav()->GetVrpnObject()->IsTracked(500)) {
     301        if (!uavVrpn->IsTracked(500)) {
    297302            Thread::Err("Optitrack, uav lost\n");
    298303            vrpnLost=true;
     
    322327    circle->SetCenter(target_2Dpos);
    323328
    324     GetUav()->GetVrpnObject()->GetPosition(uav_pos);
     329    uavVrpn->GetPosition(uav_pos);
    325330    uav_pos.To2Dxy(uav_2Dpos);
    326331    circle->StartTraj(uav_2Dpos,1);
     
    347352    }
    348353
    349     GetUav()->GetVrpnObject()->GetEuler(vrpn_euler);
     354    uavVrpn->GetEuler(vrpn_euler);
    350355    yaw_hold=vrpn_euler.yaw;
    351356
    352     GetUav()->GetVrpnObject()->GetPosition(vrpn_pos);
     357    uavVrpn->GetPosition(vrpn_pos);
    353358    vrpn_pos.To2Dxy(pos_hold);
    354359
Note: See TracChangeset for help on using the changeset viewer.