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

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

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