source: flair-src/tags/latest/demos/DemoGps/uav/src/DemoGps.cpp@ 435

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