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

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

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