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

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

singleton manager

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