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

Last change on this file since 315 was 314, checked in by Sanahuja Guillaume, 6 years ago

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