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

Last change on this file since 405 was 314, checked in by Sanahuja Guillaume, 4 years ago

maj joystick

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(GetTargetController()->IsButtonPressed(9) && GetTargetController()->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.