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

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

maj joystick

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