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

Last change on this file since 222 was 186, checked in by Sanahuja Guillaume, 7 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.