source: flair-src/branches/sanscv/demos/SimpleFleet/uav/src/SimpleFleet.cpp@ 411

Last change on this file since 411 was 324, checked in by Sanahuja Guillaume, 5 years ago

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