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

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

m

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