[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 |
|
---|
| 34 | using namespace std;
|
---|
| 35 | using namespace flair::core;
|
---|
| 36 | using namespace flair::gui;
|
---|
| 37 | using namespace flair::sensor;
|
---|
| 38 | using namespace flair::filter;
|
---|
| 39 | using namespace flair::meta;
|
---|
| 40 |
|
---|
[122] | 41 | DemoGps::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 |
|
---|
| 74 | DemoGps::~DemoGps() {
|
---|
| 75 | }
|
---|
| 76 |
|
---|
| 77 | AhrsData *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 |
|
---|
| 99 | void 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 |
|
---|
| 139 | void 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 |
|
---|
| 156 | void DemoGps::ExtraSecurityCheck(void) {
|
---|
| 157 |
|
---|
| 158 | }
|
---|
| 159 |
|
---|
| 160 | void 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 |
|
---|
| 169 | void 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 |
|
---|
| 181 | void 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 |
|
---|
| 202 | void DemoGps::StopCircle(void) {
|
---|
| 203 | circle->FinishTraj();
|
---|
| 204 | //GetJoystick()->Rumble(0x70);
|
---|
| 205 | Thread::Info("DemoGps: finishing circle\n");
|
---|
| 206 | }
|
---|
| 207 |
|
---|
| 208 | void 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 | }
|
---|