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

Last change on this file since 432 was 432, checked in by Sanahuja Guillaume, 5 months ago

add AltitudeSensor? class
failsafe altitude sensor in changeable

File size: 9.3 KB
Line 
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"
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 <FrameworkManager.h>
22#include <VrpnClient.h>
23#include <MetaVrpnObject.h>
24#include <TrajectoryGenerator2DCircle.h>
25#include <Matrix.h>
26#include <cmath>
27#include <Tab.h>
28#include <Pid.h>
29#include <Ahrs.h>
30#include <AhrsData.h>
31
32using namespace std;
33using namespace flair::core;
34using namespace flair::gui;
35using namespace flair::sensor;
36using namespace flair::filter;
37using namespace flair::meta;
38
39CircleFollower::CircleFollower(TargetController *controller): UavStateMachine(controller), behaviourMode(BehaviourMode_t::Default), vrpnLost(false) {
40    Uav* uav=GetUav();
41
42    VrpnClient* vrpnclient=new VrpnClient("vrpn", uav->GetDefaultVrpnAddress(),80,uav->GetDefaultVrpnConnectionType());
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) {
51        uavVrpn = new MetaVrpnObject(uav->ObjectName());
52        targetVrpn=new MetaVrpnObject("target");
53    }
54   
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   
60    getFrameworkManager()->AddDeviceToLog(uavVrpn);
61    getFrameworkManager()->AddDeviceToLog(targetVrpn);
62    vrpnclient->Start();
63   
64    uav->GetAhrs()->YawPlot()->AddCurve(uavVrpn->State()->Element(2),DataPlot::Green);
65                                                                                                                                 
66    startCircle=new PushButton(GetButtonsLayout()->NewRow(),"start_circle");
67    stopCircle=new PushButton(GetButtonsLayout()->LastRowLastCol(),"stop_circle");
68    positionHold=new PushButton(GetButtonsLayout()->LastRowLastCol(),"position hold");
69
70    circle=new TrajectoryGenerator2DCircle(vrpnclient->GetLayout()->NewRow(),"circle");
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");
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
94                Quaternion vrpnQuaternion;
95    uavVrpn->GetQuaternion(vrpnQuaternion);
96
97    //get roll, pitch and w from imu
98    Quaternion ahrsQuaternion;
99    Vector3Df ahrsAngularSpeed;
100    GetDefaultOrientation()->GetQuaternionAndAngularRates(ahrsQuaternion, ahrsAngularSpeed);
101
102    Euler ahrsEuler=ahrsQuaternion.ToEuler();
103    ahrsEuler.yaw=vrpnQuaternion.ToEuler().yaw;
104    Quaternion mixQuaternion=ahrsEuler.ToQuaternion();
105
106    customOrientation->SetQuaternionAndAngularRates(mixQuaternion,ahrsAngularSpeed);
107
108    return customOrientation;
109}
110
111void CircleFollower::AltitudeValues(float &z,float &dz) const{
112    Vector3Df uav_pos,uav_vel;
113
114    uavVrpn->GetPosition(uav_pos);
115    uavVrpn->GetSpeed(uav_vel);
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) {
122    Vector2Df pos_err, vel_err; // in Uav coordinate system
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
138    customReferenceOrientation->SetQuaternionAndAngularRates(refAngles.ToQuaternion(),Vector3Df(0,0,0));
139
140    return customReferenceOrientation;
141}
142
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
146
147    uavVrpn->GetPosition(uav_pos);
148    uavVrpn->GetSpeed(uav_vel);
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
158        Vector3Df target_pos;
159        Vector2Df circle_pos,circle_vel;
160        Vector2Df target_2Dpos;
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        }
211        if (!uavVrpn->IsTracked(500)) {
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    }
227    if(positionHold->Clicked() && (behaviourMode==BehaviourMode_t::Default)) {
228        VrpnPositionHold();
229    }
230}
231
232void CircleFollower::ExtraCheckJoystick(void) {
233    //R1 and Circle
234    if(GetTargetController()->IsButtonPressed(9) && GetTargetController()->IsButtonPressed(4) && (behaviourMode!=BehaviourMode_t::Circle)) {
235        StartCircle();
236    }
237
238    //R1 and Cross
239    if(GetTargetController()->IsButtonPressed(9) && GetTargetController()->IsButtonPressed(5) && (behaviourMode==BehaviourMode_t::Circle)) {
240        StopCircle();
241    }
242   
243    //R1 and Square
244    if(GetTargetController()->IsButtonPressed(9) && GetTargetController()->IsButtonPressed(2) && (behaviourMode==BehaviourMode_t::Default)) {
245        VrpnPositionHold();
246    }
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    }
256    Vector3Df uav_pos,target_pos;
257    Vector2Df uav_2Dpos,target_2Dpos;
258
259    targetVrpn->GetPosition(target_pos);
260    target_pos.To2Dxy(target_2Dpos);
261    circle->SetCenter(target_2Dpos);
262
263    uavVrpn->GetPosition(uav_pos);
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) {
279                Quaternion vrpnQuaternion;
280    uavVrpn->GetQuaternion(vrpnQuaternion);
281                yawHold=vrpnQuaternion.ToEuler().yaw;
282
283    Vector3Df vrpnPosition;
284    uavVrpn->GetPosition(vrpnPosition);
285    vrpnPosition.To2Dxy(posHold);
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.