source: flair-src/trunk/demos/MixedReality/virtual/uav/src/App.cpp @ 293

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

folder reorganization

File size: 5.3 KB
Line 
1//  created:    2019/01/09
2//  filename:   App.cpp
3//
4//  author:     Guillaume Sanahuja
5//              Copyright Heudiasyc UMR UTC/CNRS 7253
6//
7//  version:    $Id: $
8//
9//  purpose:    mixed reality demo, virtual uav side
10//
11//
12/*********************************************************************/
13
14#include "App.h"
15#include <TargetController.h>
16#include <Uav.h>
17#include <GridLayout.h>
18#include <PushButton.h>
19#include <DataPlot1D.h>
20#include <MetaDualShock3.h>
21#include <FrameworkManager.h>
22#include <VrpnClient.h>
23#include <MetaVrpnObject.h>
24#include <Matrix.h>
25#include <Tab.h>
26#include <Pid.h>
27#include <Ahrs.h>
28#include <AhrsData.h>
29
30using namespace std;
31using namespace flair::core;
32using namespace flair::gui;
33using namespace flair::sensor;
34using namespace flair::filter;
35using namespace flair::meta;
36
37App::App(TargetController *controller): UavStateMachine(controller), behaviourMode(BehaviourMode_t::Default), vrpnLost(false) {
38    Uav* uav=GetUav();
39
40    VrpnClient* vrpnclient=new VrpnClient("vrpn", "127.0.0.1:3884",80);//local ip, and specific port for simulator_simu's vrpn
41    uavVrpn = new MetaVrpnObject(uav->ObjectName());
42    getFrameworkManager()->AddDeviceToLog(uavVrpn);
43    uav->GetAhrs()->YawPlot()->AddCurve(uavVrpn->State()->Element(2),DataPlot::Green);
44                                                                                                                                 
45    positionHold=new PushButton(GetButtonsLayout()->LastRowLastCol(),"position hold");
46
47                vrpnclient->Start();
48
49    uX=new Pid(setupLawTab->At(1,0),"u_x");
50    uX->UseDefaultPlot(graphLawTab->NewRow());
51    uY=new Pid(setupLawTab->At(1,1),"u_y");
52    uY->UseDefaultPlot(graphLawTab->LastRowLastCol());
53
54    customReferenceOrientation= new AhrsData(this,"reference");
55    uav->GetAhrs()->AddPlot(customReferenceOrientation,DataPlot::Yellow);
56    AddDataToControlLawLog(customReferenceOrientation);
57
58    customOrientation=new AhrsData(this,"orientation");
59}
60
61App::~App() {
62}
63
64const AhrsData *App::GetOrientation(void) const {
65    //get yaw from vrpn
66                Quaternion vrpnQuaternion;
67    uavVrpn->GetQuaternion(vrpnQuaternion);
68
69    //get roll, pitch and w from imu
70    Quaternion ahrsQuaternion;
71    Vector3Df ahrsAngularSpeed;
72    GetDefaultOrientation()->GetQuaternionAndAngularRates(ahrsQuaternion, ahrsAngularSpeed);
73
74    Euler ahrsEuler=ahrsQuaternion.ToEuler();
75    ahrsEuler.yaw=vrpnQuaternion.ToEuler().yaw;
76    Quaternion mixQuaternion=ahrsEuler.ToQuaternion();
77
78    customOrientation->SetQuaternionAndAngularRates(mixQuaternion,ahrsAngularSpeed);
79
80    return customOrientation;
81}
82
83void App::AltitudeValues(float &z,float &dz) const{
84    Vector3Df uav_pos,uav_vel;
85
86    uavVrpn->GetPosition(uav_pos);
87    uavVrpn->GetSpeed(uav_vel);
88    //z and dz must be in uav's frame
89    z=-uav_pos.z;
90    dz=-uav_vel.z;
91}
92
93AhrsData *App::GetReferenceOrientation(void) {
94    Vector2Df pos_err, vel_err; // in Uav coordinate system
95    float yaw_ref;
96    Euler refAngles;
97
98    PositionValues(pos_err, vel_err, yaw_ref);
99
100    refAngles.yaw=yaw_ref;
101
102    uX->SetValues(pos_err.x, vel_err.x);
103    uX->Update(GetTime());
104    refAngles.pitch=uX->Output();
105
106    uY->SetValues(pos_err.y, vel_err.y);
107    uY->Update(GetTime());
108    refAngles.roll=-uY->Output();
109
110    customReferenceOrientation->SetQuaternionAndAngularRates(refAngles.ToQuaternion(),Vector3Df(0,0,0));
111
112    return customReferenceOrientation;
113}
114
115void App::PositionValues(Vector2Df &pos_error,Vector2Df &vel_error,float &yaw_ref) {
116    Vector3Df uav_pos,uav_vel; // in VRPN coordinate system
117    Vector2Df uav_2Dpos,uav_2Dvel; // in VRPN coordinate system
118
119    uavVrpn->GetPosition(uav_pos);
120    uavVrpn->GetSpeed(uav_vel);
121
122    uav_pos.To2Dxy(uav_2Dpos);
123    uav_vel.To2Dxy(uav_2Dvel);
124
125    pos_error=uav_2Dpos-posHold;
126    vel_error=uav_2Dvel;
127    yaw_ref=yawHold;
128   
129    //error in uav frame
130    Quaternion currentQuaternion=GetCurrentQuaternion();
131    Euler currentAngles;//in vrpn frame
132    currentQuaternion.ToEuler(currentAngles);
133    pos_error.Rotate(-currentAngles.yaw);
134    vel_error.Rotate(-currentAngles.yaw);
135}
136
137void App::SignalEvent(Event_t event) {
138    UavStateMachine::SignalEvent(event);
139    switch(event) {
140    case Event_t::TakingOff:
141        behaviourMode=BehaviourMode_t::Default;
142        vrpnLost=false;
143        break;
144    case Event_t::EnteringControlLoop:
145        break;
146    case Event_t::EnteringFailSafeMode:
147        behaviourMode=BehaviourMode_t::Default;
148        break;
149    }
150}
151
152void App::ExtraSecurityCheck(void) {
153    if ((!vrpnLost) && (behaviourMode==BehaviourMode_t::PositionHold)) {
154        if (!uavVrpn->IsTracked(500)) {
155            Thread::Err("VRPN, uav lost\n");
156            vrpnLost=true;
157            EnterFailSafeMode();
158            Land();
159        }
160    }
161}
162
163void App::ExtraCheckPushButton(void) {
164    if(positionHold->Clicked() && (behaviourMode==BehaviourMode_t::Default)) {
165        VrpnPositionHold();
166    }
167}
168
169void App::ExtraCheckJoystick(void) {
170    //R1 and Square
171    if(GetJoystick()->IsButtonPressed(9) && GetJoystick()->IsButtonPressed(2) && (behaviourMode==BehaviourMode_t::Default)) {
172        VrpnPositionHold();
173    }
174}
175
176void App::VrpnPositionHold(void) {
177                Quaternion vrpnQuaternion;
178    uavVrpn->GetQuaternion(vrpnQuaternion);
179                yawHold=vrpnQuaternion.ToEuler().yaw;
180
181    Vector3Df vrpnPosition;
182    uavVrpn->GetPosition(vrpnPosition);
183    vrpnPosition.To2Dxy(posHold);
184
185    uX->Reset();
186    uY->Reset();
187    behaviourMode=BehaviourMode_t::PositionHold;
188    SetOrientationMode(OrientationMode_t::Custom);
189    Thread::Info("App: holding position\n");
190}
Note: See TracBrowser for help on using the repository browser.