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

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

cosmetic changes

File size: 6.3 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
[159]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");
[89]47
[159]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);
[89]51
[159]52  gps->UseDefaultPlot();
53  getFrameworkManager()->AddDeviceToLog(gps);
54  gps->Start();
[89]55
[159]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");*/
[89]64
[159]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());
[89]69
[159]70  customReferenceOrientation = new AhrsData(this, "reference");
71  uav->GetAhrs()->AddPlot(customReferenceOrientation, DataPlot::Yellow);
72  AddDataToControlLawLog(customReferenceOrientation);
73
74  customOrientation = new AhrsData(this, "orientation");
[89]75}
76
77DemoGps::~DemoGps() {
78}
79
[159]80AhrsData* DemoGps::GetReferenceOrientation(void) {
81  Vector2D pos_err, vel_err; // in Uav coordinate system
82  float yaw_ref;
83  Euler refAngles;
[89]84
[159]85  PositionValues(pos_err, vel_err, yaw_ref);
[89]86
[159]87  refAngles.yaw = yaw_ref;
[89]88
[159]89  uX->SetValues(pos_err.x, vel_err.x);
90  uX->Update(GetTime());
91  refAngles.pitch = uX->Output();
[89]92
[159]93  uY->SetValues(pos_err.y, vel_err.y);
94  uY->Update(GetTime());
95  refAngles.roll = -uY->Output();
[89]96
[159]97  customReferenceOrientation->SetQuaternionAndAngularRates(refAngles.ToQuaternion(), Vector3D(0, 0, 0));
[89]98
[159]99  return customReferenceOrientation;
[89]100}
101
[159]102void DemoGps::PositionValues(Vector2D& pos_error, Vector2D& vel_error, float& yaw_ref) {
103  Vector3D uav_pos, uav_vel;
104  Vector2D uav_2Dpos, uav_2Dvel;
[89]105
[159]106  // TODO GPS position and circle center
107  // GetUav()->GetVrpnObject()->GetPosition(uav_pos);
108  // GetUav()->GetVrpnObject()->GetSpeed(uav_vel);
[89]109
[159]110  uav_pos.To2Dxy(uav_2Dpos);
111  uav_vel.To2Dxy(uav_2Dvel);
[89]112
[159]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    Vector2D circle_pos, circle_vel;
119    Vector2D target_2Dpos;
[89]120
[159]121    circle->SetCenter(target_2Dpos);
[89]122
[159]123    // circle reference
124    circle->Update(GetTime());
125    circle->GetPosition(circle_pos);
126    circle->GetSpeed(circle_vel);
[89]127
[159]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  }
[89]133
[159]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);
[89]140}
141
142void DemoGps::SignalEvent(Event_t event) {
[159]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();
[89]151    }
[159]152    break;
153  case Event_t::EnteringFailSafeMode:
154    behaviourMode = BehaviourMode_t::Default;
155    break;
156  }
[89]157}
158
159void DemoGps::ExtraSecurityCheck(void) {
160}
161
162void DemoGps::ExtraCheckPushButton(void) {
[159]163  if(startCircle->Clicked() && (behaviourMode != BehaviourMode_t::Circle)) {
164    StartCircle();
165  }
166  if(stopCircle->Clicked() && (behaviourMode == BehaviourMode_t::Circle)) {
167    StopCircle();
168  }
[89]169}
170
171void DemoGps::ExtraCheckJoystick(void) {
[159]172  // R1 and Circle
173  if(GetJoystick()->IsButtonPressed(9) && GetJoystick()->IsButtonPressed(4) &&
174     (behaviourMode != BehaviourMode_t::Circle)) {
175    StartCircle();
176  }
[89]177
[159]178  // R1 and Cross
179  if(GetJoystick()->IsButtonPressed(9) && GetJoystick()->IsButtonPressed(5) &&
180     (behaviourMode == BehaviourMode_t::Circle)) {
181    StopCircle();
182  }
[89]183}
184
185void DemoGps::StartCircle(void) {
[159]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  Vector3D uav_pos;
193  Vector2D uav_2Dpos, target_2Dpos;
[89]194
[159]195  circle->SetCenter(target_2Dpos);
[89]196
[159]197  // todo get uav and circle pos by gps
198  uav_pos.To2Dxy(uav_2Dpos);
199  circle->StartTraj(uav_2Dpos);
[89]200
[159]201  uX->Reset();
202  uY->Reset();
203  behaviourMode = BehaviourMode_t::Circle;
[89]204}
205
206void DemoGps::StopCircle(void) {
[159]207  circle->FinishTraj();
208  // GetJoystick()->Rumble(0x70);
209  Thread::Info("DemoGps: finishing circle\n");
[89]210}
211
212void DemoGps::GpsPositionHold(void) {
213
[159]214  // tood set yawHold and posHold
[89]215
[159]216  uX->Reset();
217  uY->Reset();
218  behaviourMode = BehaviourMode_t::PositionHold;
219  SetOrientationMode(OrientationMode_t::Custom);
220  Thread::Info("DemoGps: holding position\n");
[89]221}
Note: See TracBrowser for help on using the repository browser.