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

Last change on this file since 431 was 431, checked in by Sanahuja Guillaume, 3 years ago

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