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

Last change on this file since 38 was 38, checked in by Bayard Gildas, 5 years ago

Modif. pour ajour manette émulée (EmulatedController?)

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