source: flair-src/trunk/demos/SimpleFleet/uav/src/SimpleFleet.cpp @ 129

Last change on this file since 129 was 129, checked in by Sanahuja Guillaume, 5 years ago

resolution bug vrpn client pas demarré

File size: 11.3 KB
Line 
1//  created:    2015/11/05
2//  filename:   SimpleFleet.cpp
3//
4//  author:     Guillaume Sanahuja
5//              Copyright Heudiasyc UMR UTC/CNRS 7253
6//
7//  version:    $Id: $
8//
9//  purpose:    demo fleet
10//
11//
12/*********************************************************************/
13
14#include "SimpleFleet.h"
15#include <TargetController.h>
16#include <Uav.h>
17#include <GridLayout.h>
18#include <PushButton.h>
19#include <DataPlot1D.h>
20#include <Ahrs.h>
21#include <MetaUsRangeFinder.h>
22#include <MetaDualShock3.h>
23#include <FrameworkManager.h>
24#include <VrpnClient.h>
25#include <MetaVrpnObject.h>
26#include <TrajectoryGenerator2DCircle.h>
27#include <Vector3D.h>
28#include <Vector2D.h>
29#include <PidThrust.h>
30#include <Euler.h>
31#include <cvmatrix.h>
32#include <AhrsData.h>
33#include <Ahrs.h>
34#include <DoubleSpinBox.h>
35#include <stdio.h>
36#include <cmath>
37#include <Tab.h>
38#include <Pid.h>
39#include <Socket.h>
40#include <string.h>
41
42#define PI ((float)3.14159265358979323846)
43
44using namespace std;
45using namespace flair::core;
46using namespace flair::gui;
47using namespace flair::sensor;
48using namespace flair::filter;
49using namespace flair::meta;
50
51
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        vrpnclient->Start();
60
61    circle=new TrajectoryGenerator2DCircle(vrpnclient->GetLayout()->NewRow(),"circle");
62    uavVrpn->xPlot()->AddCurve(circle->Matrix()->Element(0,0),0,0,255);
63    uavVrpn->yPlot()->AddCurve(circle->Matrix()->Element(0,1),0,0,255);
64    uavVrpn->VxPlot()->AddCurve(circle->Matrix()->Element(1,0),0,0,255);
65    uavVrpn->VyPlot()->AddCurve(circle->Matrix()->Element(1,1),0,0,255);
66
67    xCircleCenter=new DoubleSpinBox(vrpnclient->GetLayout()->NewRow(),"x circle center"," m",-5,5,0.1,1,0);
68    yCircleCenter=new DoubleSpinBox(vrpnclient->GetLayout()->NewRow(),"y circle center"," m",-5,5,0.1,1,0);
69    yDisplacement=new DoubleSpinBox(vrpnclient->GetLayout()->NewRow(),"y displacement"," m",0,2,0.1,1,0);
70
71    //parent->AddDeviceToLog(Uz());
72
73    u_x=new Pid(setupLawTab->At(1,0),"u_x");
74    u_x->UseDefaultPlot(graphLawTab->NewRow());
75    u_y=new Pid(setupLawTab->At(1,1),"u_y");
76    u_y->UseDefaultPlot(graphLawTab->LastRowLastCol());
77
78    message=new Socket(uav,"Message",broadcast,true);
79
80    customReferenceOrientation= new AhrsData(this,"reference");
81    uav->GetAhrs()->AddPlot(customReferenceOrientation,DataPlot::Yellow);
82    AddDataToControlLawLog(customReferenceOrientation);
83
84    customOrientation=new AhrsData(this,"orientation");
85/*
86    //check init conditions
87    Vector3D uav_pos;
88    Euler vrpn_euler;
89    GetVrpnObject()->GetPosition(uav_pos);
90    GetVrpnObject()->GetEuler(vrpn_euler);
91
92    if(name=="x8_0") {
93        //x8_0 should be on the left, with 0 yaw
94        if(uav_pos.y>yCircleCenter->Value() || vrpn_euler.yaw>20 || vrpn_euler.yaw<-20) Thread::Err("wrong init position\n");
95    }
96    if(name=="x8_1") {
97        //x8_1 should be on the right, with 180 yaw
98        if(uav_pos.y<yCircleCenter->Value() || (vrpn_euler.yaw<160 && vrpn_euler.yaw>-160)) Thread::Err("wrong init position %f %f\n",yCircleCenter->Value(),vrpn_euler.yaw);
99    }
100    */
101}
102
103SimpleFleet::~SimpleFleet() {
104}
105
106const AhrsData *SimpleFleet::GetOrientation(void) const {
107    //get yaw from vrpn
108    Euler vrpnEuler;
109    uavVrpn->GetEuler(vrpnEuler);
110
111    //get roll, pitch and w from imu
112    Quaternion ahrsQuaternion;
113    Vector3D ahrsAngularSpeed;
114    GetDefaultOrientation()->GetQuaternionAndAngularRates(ahrsQuaternion, ahrsAngularSpeed);
115
116    Euler ahrsEuler=ahrsQuaternion.ToEuler();
117    ahrsEuler.yaw=vrpnEuler.yaw;
118    Quaternion mixQuaternion=ahrsEuler.ToQuaternion();
119
120    customOrientation->SetQuaternionAndAngularRates(mixQuaternion,ahrsAngularSpeed);
121
122    return customOrientation;
123}
124
125void SimpleFleet::AltitudeValues(float &z,float &dz) const {
126    Vector3D uav_pos,uav_vel;
127
128    uavVrpn->GetPosition(uav_pos);
129    uavVrpn->GetSpeed(uav_vel);
130    //z and dz must be in uav's frame
131    z=-uav_pos.z;
132    dz=-uav_vel.z;
133}
134
135const AhrsData *SimpleFleet::GetReferenceOrientation(void) {
136    Vector2D pos_err, vel_err; // in uav coordinate system
137    float yaw_ref;
138    Euler refAngles;
139
140    PositionValues(pos_err, vel_err, yaw_ref);
141
142    refAngles.yaw=yaw_ref;
143
144    u_x->SetValues(pos_err.x, vel_err.x);
145    u_x->Update(GetTime());
146    refAngles.pitch=u_x->Output();
147
148    u_y->SetValues(pos_err.y, vel_err.y);
149    u_y->Update(GetTime());
150    refAngles.roll=-u_y->Output();
151
152    customReferenceOrientation->SetQuaternionAndAngularRates(refAngles.ToQuaternion(),Vector3D(0,0,0));
153
154    return customReferenceOrientation;
155}
156
157void SimpleFleet::PositionValues(Vector2D &pos_error,Vector2D &vel_error,float &yaw_ref) {
158    Vector3D uav_pos,uav_vel; // in VRPN coordinate system
159    Vector2D uav_2Dpos,uav_2Dvel; // in VRPN coordinate system
160    Euler vrpn_euler; // in VRPN coordinate system
161
162    uavVrpn->GetPosition(uav_pos);
163    uavVrpn->GetSpeed(uav_vel);
164    uavVrpn->GetEuler(vrpn_euler);
165
166    uav_pos.To2Dxy(uav_2Dpos);
167    uav_vel.To2Dxy(uav_2Dvel);
168
169    if (behaviourMode==BehaviourMode_t::PositionHold1 || behaviourMode==BehaviourMode_t::PositionHold2
170        || behaviourMode==BehaviourMode_t::PositionHold3 || behaviourMode==BehaviourMode_t::PositionHold4) {
171        pos_error=uav_2Dpos-pos_hold;
172        vel_error=uav_2Dvel;
173        yaw_ref=yaw_hold;
174    } else { //Circle
175        Vector2D circle_pos,circle_vel;
176        Vector2D target_2Dpos;
177
178        //circle center
179        target_2Dpos.x=xCircleCenter->Value();
180        target_2Dpos.y=yCircleCenter->Value();
181        circle->SetCenter(target_2Dpos);
182
183        //circle reference
184        circle->Update(GetTime());
185        circle->GetPosition(circle_pos);
186        circle->GetSpeed(circle_vel);
187
188        //error in optitrack frame
189        pos_error=uav_2Dpos-circle_pos;
190        vel_error=uav_2Dvel-circle_vel;
191        yaw_ref=PI/2+atan2(uav_pos.y-target_2Dpos.y,uav_pos.x-target_2Dpos.x);
192    }
193    //error in uav frame
194    Quaternion currentQuaternion=GetCurrentQuaternion();
195    Euler currentAngles;//in vrpn frame
196    currentQuaternion.ToEuler(currentAngles);
197    pos_error.Rotate(-currentAngles.yaw);
198    vel_error.Rotate(-currentAngles.yaw);
199}
200
201void SimpleFleet::SignalEvent(Event_t event) {
202    UavStateMachine::SignalEvent(event);
203
204    switch(event) {
205    case Event_t::EmergencyStop:
206        message->SendMessage("EmergencyStop");
207        break;
208    case Event_t::TakingOff:
209        //behaviourMode=BehaviourMode_t::Default;
210        message->SendMessage("TakeOff");
211        VrpnPositionHold();
212        behaviourMode=BehaviourMode_t::PositionHold1;
213        break;
214    case Event_t::StartLanding:
215        VrpnPositionHold();
216        behaviourMode=BehaviourMode_t::PositionHold4;
217        message->SendMessage("Landing");
218        break;
219    case Event_t::EnteringControlLoop:
220        CheckMessages();
221        if ((behaviourMode==BehaviourMode_t::Circle1) && (!circle->IsRunning())) {
222            VrpnPositionHold();
223            behaviourMode=BehaviourMode_t::PositionHold2;
224            if(pos_hold.y<0) {
225                pos_hold.y-=yDisplacement->Value();
226            } else {
227                pos_hold.y+=yDisplacement->Value();
228            }
229            posWait=GetTime();
230            Printf("Circle1 -> PositionHold2\n");
231        }
232        if (behaviourMode==BehaviourMode_t::PositionHold2 && GetTime()>(posWait+3*(Time)1000000000)) {
233            behaviourMode=BehaviourMode_t::PositionHold3;
234            if(pos_hold.y<0) {
235                pos_hold.y+=yDisplacement->Value();
236            } else {
237                pos_hold.y-=yDisplacement->Value();
238            }
239            posWait=GetTime();
240            Printf("PositionHold2 -> PositionHold3\n");
241        }
242        if (behaviourMode==BehaviourMode_t::PositionHold3 && GetTime()>(posWait+3*(Time)1000000000)) {
243            behaviourMode=BehaviourMode_t::Circle2;
244            StartCircle();
245            Printf("PositionHold3 -> Circle2\n");
246        }
247        if ((behaviourMode==BehaviourMode_t::Circle2) && (!circle->IsRunning())) {
248            Printf("Circle2 -> Land\n");
249            behaviourMode=BehaviourMode_t::PositionHold4;
250            Land();
251        }
252
253        break;
254    case Event_t::EnteringFailSafeMode:
255        behaviourMode=BehaviourMode_t::Default;
256        break;
257    case Event_t::ZTrajectoryFinished:
258        Printf("PositionHold1 -> Circle1\n");
259        StartCircle();
260        behaviourMode=BehaviourMode_t::Circle1;
261        break;
262    }
263}
264
265void SimpleFleet::CheckMessages(void) {
266    char msg[64];
267    char src[64];
268    size_t src_size=sizeof(src);
269    while(message->RecvMessage(msg,sizeof(msg),TIME_NONBLOCK,src,&src_size)>0) {
270        //printf("%s %s\n",GetUav()->ObjectName().c_str(),src);
271        if(strcmp(src,GetUav()->ObjectName().c_str())!=0) {
272            /*
273            if(strcmp(msg,"StopMotors")==0 && orientation_state!=OrientationState_t::Stopped)
274            {
275                joy->FlashLed(DualShock3::led1,10,10);
276                joy->Rumble(0x70);
277                GetBldc()->SetEnabled(false);
278                GetUavMultiplex()->UnlockUserInterface();
279                altitude_state=AltitudeState_t::Stopped;
280                orientation_state=OrientationState_t::Stopped;
281                GetAhrs()->UnlockUserInterface();
282            }
283*/
284            if(strcmp(msg,"TakeOff")==0) {
285                Printf("TakeOff fleet\n");
286                TakeOff();
287            }
288            if(strcmp(msg,"Landing")==0) {
289                Printf("Landing fleet\n");
290                Land();
291            }
292            if(strcmp(msg,"EmergencyStop")==0) {
293                Printf("EmergencyStop fleet\n");
294                EmergencyStop();
295            }
296        }
297    }
298}
299
300void SimpleFleet::ExtraSecurityCheck(void) {
301    if (!vrpnLost && behaviourMode!=BehaviourMode_t::Default) {
302        if (!uavVrpn->IsTracked(500)) {
303            Thread::Err("Optitrack, uav lost\n");
304            vrpnLost=true;
305            EnterFailSafeMode();
306            Land();
307        }
308    }
309}
310
311void SimpleFleet::ExtraCheckJoystick(void) {
312
313}
314
315void SimpleFleet::StartCircle(void) {
316    if (SetOrientationMode(OrientationMode_t::Custom)) {
317        Thread::Info("Demo flotte: start circle\n");
318    } else {
319        Thread::Warn("Demo flotte: could not start circle\n");
320        return;
321    }
322    Vector3D uav_pos;
323    Vector2D uav_2Dpos,target_2Dpos;
324
325    //circle center
326    target_2Dpos.x=xCircleCenter->Value();
327    target_2Dpos.y=yCircleCenter->Value();
328    circle->SetCenter(target_2Dpos);
329
330    uavVrpn->GetPosition(uav_pos);
331    uav_pos.To2Dxy(uav_2Dpos);
332    circle->StartTraj(uav_2Dpos,1);
333
334    u_x->Reset();
335    u_y->Reset();
336}
337
338void SimpleFleet::StopCircle(void) {
339    circle->FinishTraj();
340    //joy->Rumble(0x70);
341    Thread::Info("Demo flotte: finishing circle\n");
342}
343
344void SimpleFleet::VrpnPositionHold(void) {
345    Euler vrpn_euler;
346    Vector3D vrpn_pos;
347
348    if (SetOrientationMode(OrientationMode_t::Custom)) {
349        Thread::Info("Demo flotte: holding position\n");
350    } else {
351        Thread::Info("Demo flotte: could not hold position\n");
352        //return;
353    }
354
355    uavVrpn->GetEuler(vrpn_euler);
356    yaw_hold=vrpn_euler.yaw;
357
358    uavVrpn->GetPosition(vrpn_pos);
359    vrpn_pos.To2Dxy(pos_hold);
360
361    u_x->Reset();
362    u_y->Reset();
363}
Note: See TracBrowser for help on using the repository browser.