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

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

CircleFollower?

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