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

Last change on this file since 170 was 168, checked in by Sanahuja Guillaume, 8 years ago

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