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

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

add logs to ugv

File size: 9.4 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);
[435]85    AddDeviceToControlLawLog(uX);
86    AddDeviceToControlLawLog(uY);
[21]87
88    customOrientation=new AhrsData(this,"orientation");
89}
90
91CircleFollower::~CircleFollower() {
92}
93
94const AhrsData *CircleFollower::GetOrientation(void) const {
95    //get yaw from vrpn
[135]96                Quaternion vrpnQuaternion;
97    uavVrpn->GetQuaternion(vrpnQuaternion);
[21]98
99    //get roll, pitch and w from imu
100    Quaternion ahrsQuaternion;
[167]101    Vector3Df ahrsAngularSpeed;
[21]102    GetDefaultOrientation()->GetQuaternionAndAngularRates(ahrsQuaternion, ahrsAngularSpeed);
103
104    Euler ahrsEuler=ahrsQuaternion.ToEuler();
[135]105    ahrsEuler.yaw=vrpnQuaternion.ToEuler().yaw;
[21]106    Quaternion mixQuaternion=ahrsEuler.ToQuaternion();
107
108    customOrientation->SetQuaternionAndAngularRates(mixQuaternion,ahrsAngularSpeed);
109
110    return customOrientation;
111}
112
[167]113void CircleFollower::AltitudeValues(float &z,float &dz) const{
114    Vector3Df uav_pos,uav_vel;
[21]115
[122]116    uavVrpn->GetPosition(uav_pos);
117    uavVrpn->GetSpeed(uav_vel);
[21]118    //z and dz must be in uav's frame
119    z=-uav_pos.z;
120    dz=-uav_vel.z;
121}
122
123AhrsData *CircleFollower::GetReferenceOrientation(void) {
[167]124    Vector2Df pos_err, vel_err; // in Uav coordinate system
[21]125    float yaw_ref;
126    Euler refAngles;
127
128    PositionValues(pos_err, vel_err, yaw_ref);
129
130    refAngles.yaw=yaw_ref;
131
132    uX->SetValues(pos_err.x, vel_err.x);
133    uX->Update(GetTime());
134    refAngles.pitch=uX->Output();
135
136    uY->SetValues(pos_err.y, vel_err.y);
137    uY->Update(GetTime());
138    refAngles.roll=-uY->Output();
139
[167]140    customReferenceOrientation->SetQuaternionAndAngularRates(refAngles.ToQuaternion(),Vector3Df(0,0,0));
[21]141
142    return customReferenceOrientation;
143}
144
[167]145void CircleFollower::PositionValues(Vector2Df &pos_error,Vector2Df &vel_error,float &yaw_ref) {
146    Vector3Df uav_pos,uav_vel; // in VRPN coordinate system
147    Vector2Df uav_2Dpos,uav_2Dvel; // in VRPN coordinate system
[21]148
[122]149    uavVrpn->GetPosition(uav_pos);
150    uavVrpn->GetSpeed(uav_vel);
[21]151
152    uav_pos.To2Dxy(uav_2Dpos);
153    uav_vel.To2Dxy(uav_2Dvel);
154
155    if (behaviourMode==BehaviourMode_t::PositionHold) {
156        pos_error=uav_2Dpos-posHold;
157        vel_error=uav_2Dvel;
158        yaw_ref=yawHold;
159    } else { //Circle
[167]160        Vector3Df target_pos;
161        Vector2Df circle_pos,circle_vel;
162        Vector2Df target_2Dpos;
[21]163
164        targetVrpn->GetPosition(target_pos);
165        target_pos.To2Dxy(target_2Dpos);
166        circle->SetCenter(target_2Dpos);
167
168        //circle reference
169        circle->Update(GetTime());
170        circle->GetPosition(circle_pos);
171        circle->GetSpeed(circle_vel);
172
173        //error in optitrack frame
174        pos_error=uav_2Dpos-circle_pos;
175        vel_error=uav_2Dvel-circle_vel;
176        yaw_ref=atan2(target_pos.y-uav_pos.y,target_pos.x-uav_pos.x);
177    }
178
179    //error in uav frame
180    Quaternion currentQuaternion=GetCurrentQuaternion();
181    Euler currentAngles;//in vrpn frame
182    currentQuaternion.ToEuler(currentAngles);
183    pos_error.Rotate(-currentAngles.yaw);
184    vel_error.Rotate(-currentAngles.yaw);
185}
186
187void CircleFollower::SignalEvent(Event_t event) {
188    UavStateMachine::SignalEvent(event);
189    switch(event) {
190    case Event_t::TakingOff:
191        behaviourMode=BehaviourMode_t::Default;
192        vrpnLost=false;
193        break;
194    case Event_t::EnteringControlLoop:
195        if ((behaviourMode==BehaviourMode_t::Circle) && (!circle->IsRunning())) {
196            VrpnPositionHold();
197        }
198        break;
199    case Event_t::EnteringFailSafeMode:
200        behaviourMode=BehaviourMode_t::Default;
201        break;
202    }
203}
204
205void CircleFollower::ExtraSecurityCheck(void) {
206    if ((!vrpnLost) && ((behaviourMode==BehaviourMode_t::Circle) || (behaviourMode==BehaviourMode_t::PositionHold))) {
207        if (!targetVrpn->IsTracked(500)) {
208            Thread::Err("VRPN, target lost\n");
209            vrpnLost=true;
210            EnterFailSafeMode();
211            Land();
212        }
[122]213        if (!uavVrpn->IsTracked(500)) {
[21]214            Thread::Err("VRPN, uav lost\n");
215            vrpnLost=true;
216            EnterFailSafeMode();
217            Land();
218        }
219    }
220}
221
222void CircleFollower::ExtraCheckPushButton(void) {
223    if(startCircle->Clicked() && (behaviourMode!=BehaviourMode_t::Circle)) {
224        StartCircle();
225    }
226    if(stopCircle->Clicked() && (behaviourMode==BehaviourMode_t::Circle)) {
227        StopCircle();
228    }
[171]229    if(positionHold->Clicked() && (behaviourMode==BehaviourMode_t::Default)) {
230        VrpnPositionHold();
231    }
[21]232}
233
234void CircleFollower::ExtraCheckJoystick(void) {
235    //R1 and Circle
[314]236    if(GetTargetController()->IsButtonPressed(9) && GetTargetController()->IsButtonPressed(4) && (behaviourMode!=BehaviourMode_t::Circle)) {
[21]237        StartCircle();
238    }
239
240    //R1 and Cross
[314]241    if(GetTargetController()->IsButtonPressed(9) && GetTargetController()->IsButtonPressed(5) && (behaviourMode==BehaviourMode_t::Circle)) {
[21]242        StopCircle();
243    }
[171]244   
245    //R1 and Square
[314]246    if(GetTargetController()->IsButtonPressed(9) && GetTargetController()->IsButtonPressed(2) && (behaviourMode==BehaviourMode_t::Default)) {
[171]247        VrpnPositionHold();
248    }
[21]249}
250
251void CircleFollower::StartCircle(void) {
252    if (SetOrientationMode(OrientationMode_t::Custom)) {
253        Thread::Info("CircleFollower: start circle\n");
254    } else {
255        Thread::Warn("CircleFollower: could not start circle\n");
256        return;
257    }
[167]258    Vector3Df uav_pos,target_pos;
259    Vector2Df uav_2Dpos,target_2Dpos;
[21]260
261    targetVrpn->GetPosition(target_pos);
262    target_pos.To2Dxy(target_2Dpos);
263    circle->SetCenter(target_2Dpos);
264
[122]265    uavVrpn->GetPosition(uav_pos);
[21]266    uav_pos.To2Dxy(uav_2Dpos);
267    circle->StartTraj(uav_2Dpos);
268
269    uX->Reset();
270    uY->Reset();
271    behaviourMode=BehaviourMode_t::Circle;
272}
273
274void CircleFollower::StopCircle(void) {
275    circle->FinishTraj();
276    //GetJoystick()->Rumble(0x70);
277    Thread::Info("CircleFollower: finishing circle\n");
278}
279
280void CircleFollower::VrpnPositionHold(void) {
[135]281                Quaternion vrpnQuaternion;
282    uavVrpn->GetQuaternion(vrpnQuaternion);
283                yawHold=vrpnQuaternion.ToEuler().yaw;
[21]284
[167]285    Vector3Df vrpnPosition;
[135]286    uavVrpn->GetPosition(vrpnPosition);
287    vrpnPosition.To2Dxy(posHold);
[21]288
289    uX->Reset();
290    uY->Reset();
291    behaviourMode=BehaviourMode_t::PositionHold;
292    SetOrientationMode(OrientationMode_t::Custom);
293    Thread::Info("CircleFollower: holding position\n");
294}
Note: See TracBrowser for help on using the repository browser.