source: flair-src/trunk/demos/CircleFollower/uav/src/CircleFollower.cpp@ 434

Last change on this file since 434 was 432, checked in by Sanahuja Guillaume, 3 years ago

add AltitudeSensor class
failsafe altitude sensor in changeable
File size: 9.3 KB
RevLine 
[21]1// created: 2011/05/01
2// filename: CircleFollower.cpp
3//
4// author: Guillaume Sanahuja
5// Copyright Heudiasyc UMR UTC/CNRS 7253
6//
7// version: $Id: $
8//
9// purpose: demo cercle avec optitrack
10//
11//
12/*********************************************************************/
13
14#include "CircleFollower.h"
[38]15#include <TargetController.h>
[21]16#include <Uav.h>
17#include <GridLayout.h>
18#include <PushButton.h>
19#include <DataPlot1D.h>
20#include <DataPlot2D.h>
21#include <FrameworkManager.h>
22#include <VrpnClient.h>
23#include <MetaVrpnObject.h>
24#include <TrajectoryGenerator2DCircle.h>
[214]25#include <Matrix.h>
[21]26#include <cmath>
27#include <Tab.h>
28#include <Pid.h>
29#include <Ahrs.h>
30#include <AhrsData.h>
31
32using namespace std;
[23]33using namespace flair::core;
34using namespace flair::gui;
[38]35using namespace flair::sensor;
[23]36using namespace flair::filter;
37using namespace flair::meta;
[21]38
[122]39CircleFollower::CircleFollower(TargetController *controller): UavStateMachine(controller), behaviourMode(BehaviourMode_t::Default), vrpnLost(false) {
40 Uav* uav=GetUav();
[129]41
[431]42 VrpnClient* vrpnclient=new VrpnClient("vrpn", uav->GetDefaultVrpnAddress(),80,uav->GetDefaultVrpnConnectionType());
[311]43
44 if(vrpnclient->ConnectionType()==VrpnClient::Xbee) {
45 uavVrpn = new MetaVrpnObject(uav->ObjectName(),(uint8_t)0);
46 targetVrpn=new MetaVrpnObject("target",1);
47 } else if (vrpnclient->ConnectionType()==VrpnClient::Vrpn) {
48 uavVrpn = new MetaVrpnObject(uav->ObjectName());
49 targetVrpn=new MetaVrpnObject("target");
50 } else if (vrpnclient->ConnectionType()==VrpnClient::VrpnLite) {
[431]51 uavVrpn = new MetaVrpnObject(uav->ObjectName());
52 targetVrpn=new MetaVrpnObject("target");
[311]53 }
54
[432]55 //set vrpn as failsafe altitude sensor for mamboedu as us in not working well for the moment
56 if(uav->GetType()=="mamboedu") {
57 SetFailSafeAltitudeSensor(uavVrpn->GetAltitudeSensor());
58 }
59
[139]60 getFrameworkManager()->AddDeviceToLog(uavVrpn);
[342]61 getFrameworkManager()->AddDeviceToLog(targetVrpn);
62 vrpnclient->Start();
63
[139]64 uav->GetAhrs()->YawPlot()->AddCurve(uavVrpn->State()->Element(2),DataPlot::Green);
[122]65
[21]66 startCircle=new PushButton(GetButtonsLayout()->NewRow(),"start_circle");
67 stopCircle=new PushButton(GetButtonsLayout()->LastRowLastCol(),"stop_circle");
[171]68 positionHold=new PushButton(GetButtonsLayout()->LastRowLastCol(),"position hold");
[21]69
[122]70 circle=new TrajectoryGenerator2DCircle(vrpnclient->GetLayout()->NewRow(),"circle");
[214]71 uavVrpn->xPlot()->AddCurve(circle->GetMatrix()->Element(0,0),DataPlot::Blue);
72 uavVrpn->yPlot()->AddCurve(circle->GetMatrix()->Element(0,1),DataPlot::Blue);
73 uavVrpn->VxPlot()->AddCurve(circle->GetMatrix()->Element(1,0),DataPlot::Blue);
74 uavVrpn->VyPlot()->AddCurve(circle->GetMatrix()->Element(1,1),DataPlot::Blue);
75 uavVrpn->XyPlot()->AddCurve(circle->GetMatrix()->Element(0,1),circle->GetMatrix()->Element(0,0),DataPlot::Blue,"circle");
[21]76
77 uX=new Pid(setupLawTab->At(1,0),"u_x");
78 uX->UseDefaultPlot(graphLawTab->NewRow());
79 uY=new Pid(setupLawTab->At(1,1),"u_y");
80 uY->UseDefaultPlot(graphLawTab->LastRowLastCol());
81
82 customReferenceOrientation= new AhrsData(this,"reference");
83 uav->GetAhrs()->AddPlot(customReferenceOrientation,DataPlot::Yellow);
84 AddDataToControlLawLog(customReferenceOrientation);
85
86 customOrientation=new AhrsData(this,"orientation");
87}
88
89CircleFollower::~CircleFollower() {
90}
91
92const AhrsData *CircleFollower::GetOrientation(void) const {
93 //get yaw from vrpn
[135]94 Quaternion vrpnQuaternion;
95 uavVrpn->GetQuaternion(vrpnQuaternion);
[21]96
97 //get roll, pitch and w from imu
98 Quaternion ahrsQuaternion;
[167]99 Vector3Df ahrsAngularSpeed;
[21]100 GetDefaultOrientation()->GetQuaternionAndAngularRates(ahrsQuaternion, ahrsAngularSpeed);
101
102 Euler ahrsEuler=ahrsQuaternion.ToEuler();
[135]103 ahrsEuler.yaw=vrpnQuaternion.ToEuler().yaw;
[21]104 Quaternion mixQuaternion=ahrsEuler.ToQuaternion();
105
106 customOrientation->SetQuaternionAndAngularRates(mixQuaternion,ahrsAngularSpeed);
107
108 return customOrientation;
109}
110
[167]111void CircleFollower::AltitudeValues(float &z,float &dz) const{
112 Vector3Df uav_pos,uav_vel;
[21]113
[122]114 uavVrpn->GetPosition(uav_pos);
115 uavVrpn->GetSpeed(uav_vel);
[21]116 //z and dz must be in uav's frame
117 z=-uav_pos.z;
118 dz=-uav_vel.z;
119}
120
121AhrsData *CircleFollower::GetReferenceOrientation(void) {
[167]122 Vector2Df pos_err, vel_err; // in Uav coordinate system
[21]123 float yaw_ref;
124 Euler refAngles;
125
126 PositionValues(pos_err, vel_err, yaw_ref);
127
128 refAngles.yaw=yaw_ref;
129
130 uX->SetValues(pos_err.x, vel_err.x);
131 uX->Update(GetTime());
132 refAngles.pitch=uX->Output();
133
134 uY->SetValues(pos_err.y, vel_err.y);
135 uY->Update(GetTime());
136 refAngles.roll=-uY->Output();
137
[167]138 customReferenceOrientation->SetQuaternionAndAngularRates(refAngles.ToQuaternion(),Vector3Df(0,0,0));
[21]139
140 return customReferenceOrientation;
141}
142
[167]143void CircleFollower::PositionValues(Vector2Df &pos_error,Vector2Df &vel_error,float &yaw_ref) {
144 Vector3Df uav_pos,uav_vel; // in VRPN coordinate system
145 Vector2Df uav_2Dpos,uav_2Dvel; // in VRPN coordinate system
[21]146
[122]147 uavVrpn->GetPosition(uav_pos);
148 uavVrpn->GetSpeed(uav_vel);
[21]149
150 uav_pos.To2Dxy(uav_2Dpos);
151 uav_vel.To2Dxy(uav_2Dvel);
152
153 if (behaviourMode==BehaviourMode_t::PositionHold) {
154 pos_error=uav_2Dpos-posHold;
155 vel_error=uav_2Dvel;
156 yaw_ref=yawHold;
157 } else { //Circle
[167]158 Vector3Df target_pos;
159 Vector2Df circle_pos,circle_vel;
160 Vector2Df target_2Dpos;
[21]161
162 targetVrpn->GetPosition(target_pos);
163 target_pos.To2Dxy(target_2Dpos);
164 circle->SetCenter(target_2Dpos);
165
166 //circle reference
167 circle->Update(GetTime());
168 circle->GetPosition(circle_pos);
169 circle->GetSpeed(circle_vel);
170
171 //error in optitrack frame
172 pos_error=uav_2Dpos-circle_pos;
173 vel_error=uav_2Dvel-circle_vel;
174 yaw_ref=atan2(target_pos.y-uav_pos.y,target_pos.x-uav_pos.x);
175 }
176
177 //error in uav frame
178 Quaternion currentQuaternion=GetCurrentQuaternion();
179 Euler currentAngles;//in vrpn frame
180 currentQuaternion.ToEuler(currentAngles);
181 pos_error.Rotate(-currentAngles.yaw);
182 vel_error.Rotate(-currentAngles.yaw);
183}
184
185void CircleFollower::SignalEvent(Event_t event) {
186 UavStateMachine::SignalEvent(event);
187 switch(event) {
188 case Event_t::TakingOff:
189 behaviourMode=BehaviourMode_t::Default;
190 vrpnLost=false;
191 break;
192 case Event_t::EnteringControlLoop:
193 if ((behaviourMode==BehaviourMode_t::Circle) && (!circle->IsRunning())) {
194 VrpnPositionHold();
195 }
196 break;
197 case Event_t::EnteringFailSafeMode:
198 behaviourMode=BehaviourMode_t::Default;
199 break;
200 }
201}
202
203void CircleFollower::ExtraSecurityCheck(void) {
204 if ((!vrpnLost) && ((behaviourMode==BehaviourMode_t::Circle) || (behaviourMode==BehaviourMode_t::PositionHold))) {
205 if (!targetVrpn->IsTracked(500)) {
206 Thread::Err("VRPN, target lost\n");
207 vrpnLost=true;
208 EnterFailSafeMode();
209 Land();
210 }
[122]211 if (!uavVrpn->IsTracked(500)) {
[21]212 Thread::Err("VRPN, uav lost\n");
213 vrpnLost=true;
214 EnterFailSafeMode();
215 Land();
216 }
217 }
218}
219
220void CircleFollower::ExtraCheckPushButton(void) {
221 if(startCircle->Clicked() && (behaviourMode!=BehaviourMode_t::Circle)) {
222 StartCircle();
223 }
224 if(stopCircle->Clicked() && (behaviourMode==BehaviourMode_t::Circle)) {
225 StopCircle();
226 }
[171]227 if(positionHold->Clicked() && (behaviourMode==BehaviourMode_t::Default)) {
228 VrpnPositionHold();
229 }
[21]230}
231
232void CircleFollower::ExtraCheckJoystick(void) {
233 //R1 and Circle
[314]234 if(GetTargetController()->IsButtonPressed(9) && GetTargetController()->IsButtonPressed(4) && (behaviourMode!=BehaviourMode_t::Circle)) {
[21]235 StartCircle();
236 }
237
238 //R1 and Cross
[314]239 if(GetTargetController()->IsButtonPressed(9) && GetTargetController()->IsButtonPressed(5) && (behaviourMode==BehaviourMode_t::Circle)) {
[21]240 StopCircle();
241 }
[171]242
243 //R1 and Square
[314]244 if(GetTargetController()->IsButtonPressed(9) && GetTargetController()->IsButtonPressed(2) && (behaviourMode==BehaviourMode_t::Default)) {
[171]245 VrpnPositionHold();
246 }
[21]247}
248
249void CircleFollower::StartCircle(void) {
250 if (SetOrientationMode(OrientationMode_t::Custom)) {
251 Thread::Info("CircleFollower: start circle\n");
252 } else {
253 Thread::Warn("CircleFollower: could not start circle\n");
254 return;
255 }
[167]256 Vector3Df uav_pos,target_pos;
257 Vector2Df uav_2Dpos,target_2Dpos;
[21]258
259 targetVrpn->GetPosition(target_pos);
260 target_pos.To2Dxy(target_2Dpos);
261 circle->SetCenter(target_2Dpos);
262
[122]263 uavVrpn->GetPosition(uav_pos);
[21]264 uav_pos.To2Dxy(uav_2Dpos);
265 circle->StartTraj(uav_2Dpos);
266
267 uX->Reset();
268 uY->Reset();
269 behaviourMode=BehaviourMode_t::Circle;
270}
271
272void CircleFollower::StopCircle(void) {
273 circle->FinishTraj();
274 //GetJoystick()->Rumble(0x70);
275 Thread::Info("CircleFollower: finishing circle\n");
276}
277
278void CircleFollower::VrpnPositionHold(void) {
[135]279 Quaternion vrpnQuaternion;
280 uavVrpn->GetQuaternion(vrpnQuaternion);
281 yawHold=vrpnQuaternion.ToEuler().yaw;
[21]282
[167]283 Vector3Df vrpnPosition;
[135]284 uavVrpn->GetPosition(vrpnPosition);
285 vrpnPosition.To2Dxy(posHold);
[21]286
287 uX->Reset();
288 uY->Reset();
289 behaviourMode=BehaviourMode_t::PositionHold;
290 SetOrientationMode(OrientationMode_t::Custom);
291 Thread::Info("CircleFollower: holding position\n");
292}
Note: See TracBrowser for help on using the repository browser.