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

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

correct socket parameter

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