source: flair-src/trunk/demos/MixedReality/uav_real/src/CircleFollower.cpp @ 292

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

add mixed reality demo

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