source: flair-src/branches/sanscv/demos/CircleFollower/uav/src/CircleFollower.cpp@ 418

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

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