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

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

modifs camera

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