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

Last change on this file since 441 was 432, checked in by Sanahuja Guillaume, 3 years 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.