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

Last change on this file since 311 was 311, checked in by Sanahuja Guillaume, 3 years ago

maj

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