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

Last change on this file since 124 was 122, checked in by Sanahuja Guillaume, 8 years ago

modifs uav vrpn i686
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(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(),10000, 80);
56 uavVrpn = new MetaVrpnObject(uav->ObjectName());
57 getFrameworkManager()->AddDeviceToLog(uavVrpn);
58 uav->GetAhrs()->YawPlot()->AddCurve(uavVrpn->State()->Element(2),DataPlot::Green);
59
60 circle=new TrajectoryGenerator2DCircle(vrpnclient->GetLayout()->NewRow(),"circle");
61 uavVrpn->xPlot()->AddCurve(circle->Matrix()->Element(0,0),0,0,255);
62 uavVrpn->yPlot()->AddCurve(circle->Matrix()->Element(0,1),0,0,255);
63 uavVrpn->VxPlot()->AddCurve(circle->Matrix()->Element(1,0),0,0,255);
64 uavVrpn->VyPlot()->AddCurve(circle->Matrix()->Element(1,1),0,0,255);
65
66 xCircleCenter=new DoubleSpinBox(vrpnclient->GetLayout()->NewRow(),"x circle center"," m",-5,5,0.1,1,0);
67 yCircleCenter=new DoubleSpinBox(vrpnclient->GetLayout()->NewRow(),"y circle center"," m",-5,5,0.1,1,0);
68 yDisplacement=new DoubleSpinBox(vrpnclient->GetLayout()->NewRow(),"y displacement"," m",0,2,0.1,1,0);
69
70 //parent->AddDeviceToLog(Uz());
71
72 u_x=new Pid(setupLawTab->At(1,0),"u_x");
73 u_x->UseDefaultPlot(graphLawTab->NewRow());
74 u_y=new Pid(setupLawTab->At(1,1),"u_y");
75 u_y->UseDefaultPlot(graphLawTab->LastRowLastCol());
76
77 message=new Socket(uav,"Message",broadcast,true);
78
79 customReferenceOrientation= new AhrsData(this,"reference");
80 uav->GetAhrs()->AddPlot(customReferenceOrientation,DataPlot::Yellow);
81 AddDataToControlLawLog(customReferenceOrientation);
82
83 customOrientation=new AhrsData(this,"orientation");
84/*
85 //check init conditions
86 Vector3D uav_pos;
87 Euler vrpn_euler;
88 GetVrpnObject()->GetPosition(uav_pos);
89 GetVrpnObject()->GetEuler(vrpn_euler);
90
91 if(name=="x8_0") {
92 //x8_0 should be on the left, with 0 yaw
93 if(uav_pos.y>yCircleCenter->Value() || vrpn_euler.yaw>20 || vrpn_euler.yaw<-20) Thread::Err("wrong init position\n");
94 }
95 if(name=="x8_1") {
96 //x8_1 should be on the right, with 180 yaw
97 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);
98 }
99 */
100}
101
102SimpleFleet::~SimpleFleet() {
103}
104
105const AhrsData *SimpleFleet::GetOrientation(void) const {
106 //get yaw from vrpn
107 Euler vrpnEuler;
108 uavVrpn->GetEuler(vrpnEuler);
109
110 //get roll, pitch and w from imu
111 Quaternion ahrsQuaternion;
112 Vector3D ahrsAngularSpeed;
113 GetDefaultOrientation()->GetQuaternionAndAngularRates(ahrsQuaternion, ahrsAngularSpeed);
114
115 Euler ahrsEuler=ahrsQuaternion.ToEuler();
116 ahrsEuler.yaw=vrpnEuler.yaw;
117 Quaternion mixQuaternion=ahrsEuler.ToQuaternion();
118
119 customOrientation->SetQuaternionAndAngularRates(mixQuaternion,ahrsAngularSpeed);
120
121 return customOrientation;
122}
123
124void SimpleFleet::AltitudeValues(float &z,float &dz) const {
125 Vector3D uav_pos,uav_vel;
126
127 uavVrpn->GetPosition(uav_pos);
128 uavVrpn->GetSpeed(uav_vel);
129 //z and dz must be in uav's frame
130 z=-uav_pos.z;
131 dz=-uav_vel.z;
132}
133
134const AhrsData *SimpleFleet::GetReferenceOrientation(void) {
135 Vector2D pos_err, vel_err; // in uav coordinate system
136 float yaw_ref;
137 Euler refAngles;
138
139 PositionValues(pos_err, vel_err, yaw_ref);
140
141 refAngles.yaw=yaw_ref;
142
143 u_x->SetValues(pos_err.x, vel_err.x);
144 u_x->Update(GetTime());
145 refAngles.pitch=u_x->Output();
146
147 u_y->SetValues(pos_err.y, vel_err.y);
148 u_y->Update(GetTime());
149 refAngles.roll=-u_y->Output();
150
151 customReferenceOrientation->SetQuaternionAndAngularRates(refAngles.ToQuaternion(),Vector3D(0,0,0));
152
153 return customReferenceOrientation;
154}
155
156void SimpleFleet::PositionValues(Vector2D &pos_error,Vector2D &vel_error,float &yaw_ref) {
157 Vector3D uav_pos,uav_vel; // in VRPN coordinate system
158 Vector2D uav_2Dpos,uav_2Dvel; // in VRPN coordinate system
159 Euler vrpn_euler; // in VRPN coordinate system
160
161 uavVrpn->GetPosition(uav_pos);
162 uavVrpn->GetSpeed(uav_vel);
163 uavVrpn->GetEuler(vrpn_euler);
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-pos_hold;
171 vel_error=uav_2Dvel;
172 yaw_ref=yaw_hold;
173 } else { //Circle
174 Vector2D circle_pos,circle_vel;
175 Vector2D 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(pos_hold.y<0) {
224 pos_hold.y-=yDisplacement->Value();
225 } else {
226 pos_hold.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(pos_hold.y<0) {
234 pos_hold.y+=yDisplacement->Value();
235 } else {
236 pos_hold.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 Vector3D uav_pos;
322 Vector2D 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 Euler vrpn_euler;
345 Vector3D vrpn_pos;
346
347 if (SetOrientationMode(OrientationMode_t::Custom)) {
348 Thread::Info("Demo flotte: holding position\n");
349 } else {
350 Thread::Info("Demo flotte: could not hold position\n");
351 //return;
352 }
353
354 uavVrpn->GetEuler(vrpn_euler);
355 yaw_hold=vrpn_euler.yaw;
356
357 uavVrpn->GetPosition(vrpn_pos);
358 vrpn_pos.To2Dxy(pos_hold);
359
360 u_x->Reset();
361 u_y->Reset();
362}
Note: See TracBrowser for help on using the repository browser.