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

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

maj imu

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