source: flair-src/branches/sanscv/demos/MixedReality/real/uav/src/CircleFollower.cpp@ 396

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

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