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

Last change on this file since 22 was 21, checked in by Sanahuja Guillaume, 9 years ago

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