source: flair-src/trunk/demos/MixedReality/real/uav/src/CircleFollower.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: 9.2 KB
Line 
1// created: 2019/01/09
2// filename: CircleFollower.cpp
3//
4// author: Guillaume Sanahuja
5// Copyright Heudiasyc UMR UTC/CNRS 7253
6//
7// version: $Id: $
8//
9// purpose: mixed reality demo, real uav side
10//
11//
12/*********************************************************************/
13
14#include "CircleFollower.h"
15#include <TargetController.h>
16#include <Uav.h>
17#include <GridLayout.h>
18#include <PushButton.h>
19#include <DataPlot1D.h>
20#include <DataPlot2D.h>
21#include <MetaDualShock3.h>
22#include <FrameworkManager.h>
23#include <VrpnClient.h>
24#include <MetaVrpnObject.h>
25#include <TrajectoryGenerator2DCircle.h>
26#include <Matrix.h>
27#include <cmath>
28#include <Tab.h>
29#include <Pid.h>
30#include <Ahrs.h>
31#include <AhrsData.h>
32
33using namespace std;
34using namespace flair::core;
35using namespace flair::gui;
36using namespace flair::sensor;
37using namespace flair::filter;
38using namespace flair::meta;
39
40CircleFollower::CircleFollower(TargetController *controller,string vrpn): UavStateMachine(controller), behaviourMode(BehaviourMode_t::Default), vrpnLost(false) {
41 Uav* uav=GetUav();
42
43 VrpnClient* realVrpnClient=new VrpnClient("real_vrpn", uav->GetDefaultVrpnAddress(),80,uav->GetDefaultVrpnConnectionType());
44 if(realVrpnClient->ConnectionType()==VrpnClient::Xbee) {
45 uavVrpn = new MetaVrpnObject(uav->ObjectName(),(uint8_t)0);
46 } else if (realVrpnClient->ConnectionType()==VrpnClient::Vrpn) {
47 uavVrpn = new MetaVrpnObject(uav->ObjectName());
48 } else if (realVrpnClient->ConnectionType()==VrpnClient::VrpnLite) {
49 uavVrpn = new MetaVrpnObject(uav->ObjectName());
50 }
51 getFrameworkManager()->AddDeviceToLog(uavVrpn);
52 uav->GetAhrs()->YawPlot()->AddCurve(uavVrpn->State()->Element(2),DataPlot::Green);
53
54 startCircle=new PushButton(GetButtonsLayout()->NewRow(),"start_circle");
55 stopCircle=new PushButton(GetButtonsLayout()->LastRowLastCol(),"stop_circle");
56 positionHold=new PushButton(GetButtonsLayout()->LastRowLastCol(),"position hold");
57
58 VrpnClient* simuVrpnClient=new VrpnClient("simu_vrpn", vrpn,80);//simu_simu vrpn server
59 targetVrpn=new MetaVrpnObject("Drone_1",simuVrpnClient);
60 simuVrpnClient->Start();
61 realVrpnClient->Start();
62
63 getFrameworkManager()->AddDeviceToLog(targetVrpn);
64
65 circle=new TrajectoryGenerator2DCircle(realVrpnClient->GetLayout()->NewRow(),"circle");
66 uavVrpn->xPlot()->AddCurve(circle->GetMatrix()->Element(0,0),DataPlot::Blue);
67 uavVrpn->yPlot()->AddCurve(circle->GetMatrix()->Element(0,1),DataPlot::Blue);
68 uavVrpn->VxPlot()->AddCurve(circle->GetMatrix()->Element(1,0),DataPlot::Blue);
69 uavVrpn->VyPlot()->AddCurve(circle->GetMatrix()->Element(1,1),DataPlot::Blue);
70 uavVrpn->XyPlot()->AddCurve(circle->GetMatrix()->Element(0,1),circle->GetMatrix()->Element(0,0),DataPlot::Blue,"circle");
71
72 uX=new Pid(setupLawTab->At(1,0),"u_x");
73 uX->UseDefaultPlot(graphLawTab->NewRow());
74 uY=new Pid(setupLawTab->At(1,1),"u_y");
75 uY->UseDefaultPlot(graphLawTab->LastRowLastCol());
76
77 customReferenceOrientation= new AhrsData(this,"reference");
78 uav->GetAhrs()->AddPlot(customReferenceOrientation,DataPlot::Yellow);
79 AddDataToControlLawLog(customReferenceOrientation);
80
81 customOrientation=new AhrsData(this,"orientation");
82}
83
84CircleFollower::~CircleFollower() {
85}
86
87const AhrsData *CircleFollower::GetOrientation(void) const {
88 //get yaw from vrpn
89 Quaternion vrpnQuaternion;
90 uavVrpn->GetQuaternion(vrpnQuaternion);
91
92 //get roll, pitch and w from imu
93 Quaternion ahrsQuaternion;
94 Vector3Df ahrsAngularSpeed;
95 GetDefaultOrientation()->GetQuaternionAndAngularRates(ahrsQuaternion, ahrsAngularSpeed);
96
97 Euler ahrsEuler=ahrsQuaternion.ToEuler();
98 ahrsEuler.yaw=vrpnQuaternion.ToEuler().yaw;
99 Quaternion mixQuaternion=ahrsEuler.ToQuaternion();
100
101 customOrientation->SetQuaternionAndAngularRates(mixQuaternion,ahrsAngularSpeed);
102
103 return customOrientation;
104}
105
106void CircleFollower::AltitudeValues(float &z,float &dz) const{
107 Vector3Df uav_pos,uav_vel;
108
109 uavVrpn->GetPosition(uav_pos);
110 uavVrpn->GetSpeed(uav_vel);
111 //z and dz must be in uav's frame
112 z=-uav_pos.z;
113 dz=-uav_vel.z;
114}
115
116AhrsData *CircleFollower::GetReferenceOrientation(void) {
117 Vector2Df pos_err, vel_err; // in Uav coordinate system
118 float yaw_ref;
119 Euler refAngles;
120
121 PositionValues(pos_err, vel_err, yaw_ref);
122
123 refAngles.yaw=yaw_ref;
124
125 uX->SetValues(pos_err.x, vel_err.x);
126 uX->Update(GetTime());
127 refAngles.pitch=uX->Output();
128
129 uY->SetValues(pos_err.y, vel_err.y);
130 uY->Update(GetTime());
131 refAngles.roll=-uY->Output();
132
133 customReferenceOrientation->SetQuaternionAndAngularRates(refAngles.ToQuaternion(),Vector3Df(0,0,0));
134
135 return customReferenceOrientation;
136}
137
138void CircleFollower::PositionValues(Vector2Df &pos_error,Vector2Df &vel_error,float &yaw_ref) {
139 Vector3Df uav_pos,uav_vel; // in VRPN coordinate system
140 Vector2Df uav_2Dpos,uav_2Dvel; // in VRPN coordinate system
141
142 uavVrpn->GetPosition(uav_pos);
143 uavVrpn->GetSpeed(uav_vel);
144
145 uav_pos.To2Dxy(uav_2Dpos);
146 uav_vel.To2Dxy(uav_2Dvel);
147
148 if (behaviourMode==BehaviourMode_t::PositionHold) {
149 pos_error=uav_2Dpos-posHold;
150 vel_error=uav_2Dvel;
151 yaw_ref=yawHold;
152 } else { //Circle
153 Vector3Df target_pos;
154 Vector2Df circle_pos,circle_vel;
155 Vector2Df target_2Dpos;
156
157 targetVrpn->GetPosition(target_pos);
158 target_pos.To2Dxy(target_2Dpos);
159 circle->SetCenter(target_2Dpos);
160
161 //circle reference
162 circle->Update(GetTime());
163 circle->GetPosition(circle_pos);
164 circle->GetSpeed(circle_vel);
165
166 //error in optitrack frame
167 pos_error=uav_2Dpos-circle_pos;
168 vel_error=uav_2Dvel-circle_vel;
169 yaw_ref=atan2(target_pos.y-uav_pos.y,target_pos.x-uav_pos.x);
170 }
171
172 //error in uav frame
173 Quaternion currentQuaternion=GetCurrentQuaternion();
174 Euler currentAngles;//in vrpn frame
175 currentQuaternion.ToEuler(currentAngles);
176 pos_error.Rotate(-currentAngles.yaw);
177 vel_error.Rotate(-currentAngles.yaw);
178}
179
180void CircleFollower::SignalEvent(Event_t event) {
181 UavStateMachine::SignalEvent(event);
182 switch(event) {
183 case Event_t::TakingOff:
184 behaviourMode=BehaviourMode_t::Default;
185 vrpnLost=false;
186 break;
187 case Event_t::EnteringControlLoop:
188 if ((behaviourMode==BehaviourMode_t::Circle) && (!circle->IsRunning())) {
189 VrpnPositionHold();
190 }
191 break;
192 case Event_t::EnteringFailSafeMode:
193 behaviourMode=BehaviourMode_t::Default;
194 break;
195 }
196}
197
198void CircleFollower::ExtraSecurityCheck(void) {
199 if ((!vrpnLost) && ((behaviourMode==BehaviourMode_t::Circle) || (behaviourMode==BehaviourMode_t::PositionHold))) {
200 if (!targetVrpn->IsTracked(500)) {
201 Thread::Err("VRPN, target lost\n");
202 vrpnLost=true;
203 EnterFailSafeMode();
204 Land();
205 }
206 if (!uavVrpn->IsTracked(500)) {
207 Thread::Err("VRPN, uav lost\n");
208 vrpnLost=true;
209 EnterFailSafeMode();
210 Land();
211 }
212 }
213}
214
215void CircleFollower::ExtraCheckPushButton(void) {
216 if(startCircle->Clicked() && (behaviourMode!=BehaviourMode_t::Circle)) {
217 StartCircle();
218 }
219 if(stopCircle->Clicked() && (behaviourMode==BehaviourMode_t::Circle)) {
220 StopCircle();
221 }
222 if(positionHold->Clicked() && (behaviourMode==BehaviourMode_t::Default)) {
223 VrpnPositionHold();
224 }
225}
226
227void CircleFollower::ExtraCheckJoystick(void) {
228 //R1 and Circle
229 if(GetTargetController()->IsButtonPressed(9) && GetTargetController()->IsButtonPressed(4) && (behaviourMode!=BehaviourMode_t::Circle)) {
230 StartCircle();
231 }
232
233 //R1 and Cross
234 if(GetTargetController()->IsButtonPressed(9) && GetTargetController()->IsButtonPressed(5) && (behaviourMode==BehaviourMode_t::Circle)) {
235 StopCircle();
236 }
237
238 //R1 and Square
239 if(GetTargetController()->IsButtonPressed(9) && GetTargetController()->IsButtonPressed(2) && (behaviourMode==BehaviourMode_t::Default)) {
240 VrpnPositionHold();
241 }
242}
243
244void CircleFollower::StartCircle(void) {
245 if (SetOrientationMode(OrientationMode_t::Custom)) {
246 Thread::Info("CircleFollower: start circle\n");
247 } else {
248 Thread::Warn("CircleFollower: could not start circle\n");
249 return;
250 }
251 Vector3Df uav_pos,target_pos;
252 Vector2Df uav_2Dpos,target_2Dpos;
253
254 targetVrpn->GetPosition(target_pos);
255 target_pos.To2Dxy(target_2Dpos);
256 circle->SetCenter(target_2Dpos);
257
258 uavVrpn->GetPosition(uav_pos);
259 uav_pos.To2Dxy(uav_2Dpos);
260 circle->StartTraj(uav_2Dpos);
261
262 uX->Reset();
263 uY->Reset();
264 behaviourMode=BehaviourMode_t::Circle;
265}
266
267void CircleFollower::StopCircle(void) {
268 circle->FinishTraj();
269 //GetJoystick()->Rumble(0x70);
270 Thread::Info("CircleFollower: finishing circle\n");
271}
272
273void CircleFollower::VrpnPositionHold(void) {
274 Quaternion vrpnQuaternion;
275 uavVrpn->GetQuaternion(vrpnQuaternion);
276 yawHold=vrpnQuaternion.ToEuler().yaw;
277
278 Vector3Df vrpnPosition;
279 uavVrpn->GetPosition(vrpnPosition);
280 vrpnPosition.To2Dxy(posHold);
281
282 uX->Reset();
283 uY->Reset();
284 behaviourMode=BehaviourMode_t::PositionHold;
285 SetOrientationMode(OrientationMode_t::Custom);
286 Thread::Info("CircleFollower: holding position\n");
287}
Note: See TracBrowser for help on using the repository browser.