source: flair-src/trunk/demos/CircleFollower/uav/src/CircleFollower.cpp@ 219

Last change on this file since 219 was 214, checked in by Sanahuja Guillaume, 7 years ago

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