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

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

modifs pour template vectors

File size: 8.4 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#include <Camera.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    uavVrpn = new MetaVrpnObject(uav->ObjectName());
46    getFrameworkManager()->AddDeviceToLog(uavVrpn);
47    uav->GetAhrs()->YawPlot()->AddCurve(uavVrpn->State()->Element(2),DataPlot::Green);
48                                                                                                                                 
49    startCircle=new PushButton(GetButtonsLayout()->NewRow(),"start_circle");
50    stopCircle=new PushButton(GetButtonsLayout()->LastRowLastCol(),"stop_circle");
51
52    if(vrpnclient->UseXbee()==true) {
53        targetVrpn=new MetaVrpnObject("target",1);
54    } else {
55        targetVrpn=new MetaVrpnObject("target");
56    }
57                vrpnclient->Start();
58
59    getFrameworkManager()->AddDeviceToLog(targetVrpn);
60
61    circle=new TrajectoryGenerator2DCircle(vrpnclient->GetLayout()->NewRow(),"circle");
62    uavVrpn->xPlot()->AddCurve(circle->Matrix()->Element(0,0),DataPlot::Blue);
63    uavVrpn->yPlot()->AddCurve(circle->Matrix()->Element(0,1),DataPlot::Blue);
64    uavVrpn->VxPlot()->AddCurve(circle->Matrix()->Element(1,0),DataPlot::Blue);
65    uavVrpn->VyPlot()->AddCurve(circle->Matrix()->Element(1,1),DataPlot::Blue);
66    uavVrpn->XyPlot()->AddCurve(circle->Matrix()->Element(0,1),circle->Matrix()->Element(0,0),DataPlot::Blue,"circle");
67
68    uX=new Pid(setupLawTab->At(1,0),"u_x");
69    uX->UseDefaultPlot(graphLawTab->NewRow());
70    uY=new Pid(setupLawTab->At(1,1),"u_y");
71    uY->UseDefaultPlot(graphLawTab->LastRowLastCol());
72
73    customReferenceOrientation= new AhrsData(this,"reference");
74    uav->GetAhrs()->AddPlot(customReferenceOrientation,DataPlot::Yellow);
75    AddDataToControlLawLog(customReferenceOrientation);
76
77    customOrientation=new AhrsData(this,"orientation");
78                getFrameworkManager()->AddDeviceToLog(uav->GetVerticalCamera());
79}
80
81CircleFollower::~CircleFollower() {
82}
83
84const AhrsData *CircleFollower::GetOrientation(void) const {
85    //get yaw from vrpn
86                Quaternion vrpnQuaternion;
87    uavVrpn->GetQuaternion(vrpnQuaternion);
88
89    //get roll, pitch and w from imu
90    Quaternion ahrsQuaternion;
91    Vector3Df ahrsAngularSpeed;
92    GetDefaultOrientation()->GetQuaternionAndAngularRates(ahrsQuaternion, ahrsAngularSpeed);
93
94    Euler ahrsEuler=ahrsQuaternion.ToEuler();
95    ahrsEuler.yaw=vrpnQuaternion.ToEuler().yaw;
96    Quaternion mixQuaternion=ahrsEuler.ToQuaternion();
97
98    customOrientation->SetQuaternionAndAngularRates(mixQuaternion,ahrsAngularSpeed);
99
100    return customOrientation;
101}
102
103void CircleFollower::AltitudeValues(float &z,float &dz) const{
104    Vector3Df uav_pos,uav_vel;
105
106    uavVrpn->GetPosition(uav_pos);
107    uavVrpn->GetSpeed(uav_vel);
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) {
114    Vector2Df pos_err, vel_err; // in Uav coordinate system
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
130    customReferenceOrientation->SetQuaternionAndAngularRates(refAngles.ToQuaternion(),Vector3Df(0,0,0));
131
132    return customReferenceOrientation;
133}
134
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
138
139    uavVrpn->GetPosition(uav_pos);
140    uavVrpn->GetSpeed(uav_vel);
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
150        Vector3Df target_pos;
151        Vector2Df circle_pos,circle_vel;
152        Vector2Df target_2Dpos;
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        }
203        if (!uavVrpn->IsTracked(500)) {
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    }
219}
220
221void CircleFollower::ExtraCheckJoystick(void) {
222    //R1 and Circle
223    if(GetJoystick()->IsButtonPressed(9) && GetJoystick()->IsButtonPressed(4) && (behaviourMode!=BehaviourMode_t::Circle)) {
224        StartCircle();
225    }
226
227    //R1 and Cross
228    if(GetJoystick()->IsButtonPressed(9) && GetJoystick()->IsButtonPressed(5) && (behaviourMode==BehaviourMode_t::Circle)) {
229        StopCircle();
230    }
231}
232
233void CircleFollower::StartCircle(void) {
234    if (SetOrientationMode(OrientationMode_t::Custom)) {
235        Thread::Info("CircleFollower: start circle\n");
236    } else {
237        Thread::Warn("CircleFollower: could not start circle\n");
238        return;
239    }
240    Vector3Df uav_pos,target_pos;
241    Vector2Df uav_2Dpos,target_2Dpos;
242
243    targetVrpn->GetPosition(target_pos);
244    target_pos.To2Dxy(target_2Dpos);
245    circle->SetCenter(target_2Dpos);
246
247    uavVrpn->GetPosition(uav_pos);
248    uav_pos.To2Dxy(uav_2Dpos);
249    circle->StartTraj(uav_2Dpos);
250
251    uX->Reset();
252    uY->Reset();
253    behaviourMode=BehaviourMode_t::Circle;
254}
255
256void CircleFollower::StopCircle(void) {
257    circle->FinishTraj();
258    //GetJoystick()->Rumble(0x70);
259    Thread::Info("CircleFollower: finishing circle\n");
260}
261
262void CircleFollower::VrpnPositionHold(void) {
263                Quaternion vrpnQuaternion;
264    uavVrpn->GetQuaternion(vrpnQuaternion);
265                yawHold=vrpnQuaternion.ToEuler().yaw;
266
267    Vector3Df vrpnPosition;
268    uavVrpn->GetPosition(vrpnPosition);
269    vrpnPosition.To2Dxy(posHold);
270
271    uX->Reset();
272    uY->Reset();
273    behaviourMode=BehaviourMode_t::PositionHold;
274    SetOrientationMode(OrientationMode_t::Custom);
275    Thread::Info("CircleFollower: holding position\n");
276}
Note: See TracBrowser for help on using the repository browser.