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

Last change on this file since 214 was 214, checked in by Sanahuja Guillaume, 4 years ago

matrix

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