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

Last change on this file was 432, checked in by Sanahuja Guillaume, 4 months ago

add AltitudeSensor? class
failsafe altitude sensor in changeable

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