source: flair-src/trunk/demos/Gps/uav/src/DemoGps.cpp @ 167

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

modifs pour template vectors

File size: 6.3 KB
Line 
1//  created:    2016/07/01
2//  filename:   DemoGps.cpp
3//
4//  author:     Guillaume Sanahuja
5//              Copyright Heudiasyc UMR UTC/CNRS 7253
6//
7//  version:    $Id: $
8//
9//  purpose:    demo GPS
10//
11//
12/*********************************************************************/
13
14#include "DemoGps.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 <TrajectoryGenerator2DCircle.h>
24#include <cvmatrix.h>
25#include <cmath>
26#include <Tab.h>
27#include <Pid.h>
28#include <Ahrs.h>
29#include <AhrsData.h>
30#include <RTDM_SerialPort.h>
31#include <Mb800.h>
32#include <SimuGps.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
41DemoGps::DemoGps(TargetController* controller)
42    : UavStateMachine(controller)
43    , behaviourMode(BehaviourMode_t::Default) {
44  Uav* uav = GetUav();
45  startCircle = new PushButton(GetButtonsLayout()->NewRow(), "start_circle");
46  stopCircle = new PushButton(GetButtonsLayout()->LastRowLastCol(), "stop_circle");
47
48  // RTDM_SerialPort *serialport=new RTDM_SerialPort(getFrameworkManager(),"gps_serial","rtser2");
49  // gps=new Mb800("gps",serialport,(NmeaGps::NMEAFlags_t)(NmeaGps::GGA|NmeaGps::VTG),40);
50  gps = new SimuGps("gps", (NmeaGps::NMEAFlags_t)(NmeaGps::GGA | NmeaGps::VTG), 0,0, 40);
51
52  gps->UseDefaultPlot();
53  getFrameworkManager()->AddDeviceToLog(gps);
54  gps->Start();
55
56  circle = new TrajectoryGenerator2DCircle(gps->GetLayout()->NewRow(), "circle");
57  // todo: add graphs in gps plot
58  /*
59  uav->GetVrpnObject()->xPlot()->AddCurve(circle->Matrix()->Element(0,0),DataPlot::Blue);
60  uav->GetVrpnObject()->yPlot()->AddCurve(circle->Matrix()->Element(0,1),DataPlot::Blue);
61  uav->GetVrpnObject()->VxPlot()->AddCurve(circle->Matrix()->Element(1,0),DataPlot::Blue);
62  uav->GetVrpnObject()->VyPlot()->AddCurve(circle->Matrix()->Element(1,1),DataPlot::Blue);
63  uav->GetVrpnObject()->XyPlot()->AddCurve(circle->Matrix()->Element(0,1),circle->Matrix()->Element(0,0),DataPlot::Blue,"circle");*/
64
65  uX = new Pid(setupLawTab->At(1, 0), "u_x");
66  uX->UseDefaultPlot(graphLawTab->NewRow());
67  uY = new Pid(setupLawTab->At(1, 1), "u_y");
68  uY->UseDefaultPlot(graphLawTab->LastRowLastCol());
69
70  customReferenceOrientation = new AhrsData(this, "reference");
71  uav->GetAhrs()->AddPlot(customReferenceOrientation, DataPlot::Yellow);
72  AddDataToControlLawLog(customReferenceOrientation);
73
74  customOrientation = new AhrsData(this, "orientation");
75}
76
77DemoGps::~DemoGps() {
78}
79
80AhrsData* DemoGps::GetReferenceOrientation(void) {
81  Vector2Df pos_err, vel_err; // in Uav coordinate system
82  float yaw_ref;
83  Euler refAngles;
84
85  PositionValues(pos_err, vel_err, yaw_ref);
86
87  refAngles.yaw = yaw_ref;
88
89  uX->SetValues(pos_err.x, vel_err.x);
90  uX->Update(GetTime());
91  refAngles.pitch = uX->Output();
92
93  uY->SetValues(pos_err.y, vel_err.y);
94  uY->Update(GetTime());
95  refAngles.roll = -uY->Output();
96
97  customReferenceOrientation->SetQuaternionAndAngularRates(refAngles.ToQuaternion(), Vector3Df(0, 0, 0));
98
99  return customReferenceOrientation;
100}
101
102void DemoGps::PositionValues(Vector2Df& pos_error, Vector2Df& vel_error, float& yaw_ref) {
103  Vector3Df uav_pos, uav_vel;
104  Vector2Df uav_2Dpos, uav_2Dvel;
105
106  // TODO GPS position and circle center
107  // GetUav()->GetVrpnObject()->GetPosition(uav_pos);
108  // GetUav()->GetVrpnObject()->GetSpeed(uav_vel);
109
110  uav_pos.To2Dxy(uav_2Dpos);
111  uav_vel.To2Dxy(uav_2Dvel);
112
113  if(behaviourMode == BehaviourMode_t::PositionHold) {
114    pos_error = uav_2Dpos - posHold;
115    vel_error = uav_2Dvel;
116    yaw_ref = yawHold;
117  } else { // Circle
118    Vector2Df circle_pos, circle_vel;
119    Vector2Df target_2Dpos;
120
121    circle->SetCenter(target_2Dpos);
122
123    // circle reference
124    circle->Update(GetTime());
125    circle->GetPosition(circle_pos);
126    circle->GetSpeed(circle_vel);
127
128    // error in optitrack frame
129    pos_error = uav_2Dpos - circle_pos;
130    vel_error = uav_2Dvel - circle_vel;
131    yaw_ref = atan2(target_2Dpos.y - uav_pos.y, target_2Dpos.x - uav_pos.x);
132  }
133
134  // error in uav frame
135  Quaternion currentQuaternion = GetCurrentQuaternion();
136  Euler currentAngles; // in vrpn frame
137  currentQuaternion.ToEuler(currentAngles);
138  pos_error.Rotate(-currentAngles.yaw);
139  vel_error.Rotate(-currentAngles.yaw);
140}
141
142void DemoGps::SignalEvent(Event_t event) {
143  UavStateMachine::SignalEvent(event);
144  switch(event) {
145  case Event_t::TakingOff:
146    behaviourMode = BehaviourMode_t::Default;
147    break;
148  case Event_t::EnteringControlLoop:
149    if((behaviourMode == BehaviourMode_t::Circle) && (!circle->IsRunning())) {
150      GpsPositionHold();
151    }
152    break;
153  case Event_t::EnteringFailSafeMode:
154    behaviourMode = BehaviourMode_t::Default;
155    break;
156  }
157}
158
159void DemoGps::ExtraSecurityCheck(void) {
160}
161
162void DemoGps::ExtraCheckPushButton(void) {
163  if(startCircle->Clicked() && (behaviourMode != BehaviourMode_t::Circle)) {
164    StartCircle();
165  }
166  if(stopCircle->Clicked() && (behaviourMode == BehaviourMode_t::Circle)) {
167    StopCircle();
168  }
169}
170
171void DemoGps::ExtraCheckJoystick(void) {
172  // R1 and Circle
173  if(GetJoystick()->IsButtonPressed(9) && GetJoystick()->IsButtonPressed(4) &&
174     (behaviourMode != BehaviourMode_t::Circle)) {
175    StartCircle();
176  }
177
178  // R1 and Cross
179  if(GetJoystick()->IsButtonPressed(9) && GetJoystick()->IsButtonPressed(5) &&
180     (behaviourMode == BehaviourMode_t::Circle)) {
181    StopCircle();
182  }
183}
184
185void DemoGps::StartCircle(void) {
186  if(SetOrientationMode(OrientationMode_t::Custom)) {
187    Thread::Info("DemoGps: start circle\n");
188  } else {
189    Thread::Warn("DemoGps: could not start circle\n");
190    return;
191  }
192  Vector3Df uav_pos;
193  Vector2Df uav_2Dpos, target_2Dpos;
194
195  circle->SetCenter(target_2Dpos);
196
197  // todo get uav and circle pos by gps
198  uav_pos.To2Dxy(uav_2Dpos);
199  circle->StartTraj(uav_2Dpos);
200
201  uX->Reset();
202  uY->Reset();
203  behaviourMode = BehaviourMode_t::Circle;
204}
205
206void DemoGps::StopCircle(void) {
207  circle->FinishTraj();
208  // GetJoystick()->Rumble(0x70);
209  Thread::Info("DemoGps: finishing circle\n");
210}
211
212void DemoGps::GpsPositionHold(void) {
213
214  // tood set yawHold and posHold
215
216  uX->Reset();
217  uY->Reset();
218  behaviourMode = BehaviourMode_t::PositionHold;
219  SetOrientationMode(OrientationMode_t::Custom);
220  Thread::Info("DemoGps: holding position\n");
221}
Note: See TracBrowser for help on using the repository browser.