source: flair-src/trunk/lib/FlairMeta/src/Uav.cpp @ 10

Last change on this file since 10 was 10, checked in by Sanahuja Guillaume, 6 years ago

lic

File size: 5.5 KB
RevLine 
[10]1// %flair:license{
2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
4// %flair:license}
[7]5//  created:    2014/06/10
6//  filename:   Uav.cpp
7//
8//  author:     Guillaume Sanahuja
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
13//  purpose:    Base class to construct sensors/actuators depending on uav type
14//
15//
16/*********************************************************************/
17
18#include "Uav.h"
19#include <FrameworkManager.h>
20#include <Tab.h>
21#include <GridLayout.h>
22#include <DataPlot1D.h>
23#include <VrpnClient.h>
24#include <Ahrs.h>
25#include <Imu.h>
26#include <UavMultiplex.h>
27#include <UsRangeFinder.h>
28#include <Bldc.h>
29#include <cvmatrix.h>
30#include "MetaUsRangeFinder.h"
31#include "MetaVrpnObject.h"
32
33using std::string;
34using namespace flair::core;
35using namespace flair::gui;
36using namespace flair::filter;
37using namespace flair::sensor;
38using namespace flair::actuator;
39
40namespace flair { namespace meta {
41
42Uav::Uav(FrameworkManager* parent,string name,UavMultiplex *multiplex): Object(parent,name) {
43    vrpnclient=NULL;
44    uav_vrpn=NULL;
45    verticalCamera=NULL;
46    this->multiplex=multiplex;
47}
48
49Uav::~Uav() {
50}
51
52void Uav::SetUsRangeFinder(const UsRangeFinder *inUs) {
53    us=(UsRangeFinder*)inUs;
54    meta_us=new MetaUsRangeFinder(us);
55    getFrameworkManager()->AddDeviceToLog(us);
56}
57
58void Uav::SetAhrs(const Ahrs *inAhrs) {
59    ahrs=(Ahrs*)inAhrs;
60    imu=(Imu*)ahrs->GetImu();
61    getFrameworkManager()->AddDeviceToLog(imu);
62}
63
64void Uav::SetBldc(const Bldc* inBldc) {
65    bldc=(Bldc*)inBldc;
66}
67
68void Uav::SetBatteryMonitor(const BatteryMonitor* inBattery) {
69    battery=(BatteryMonitor*)inBattery;
70}
71
72void Uav::SetMultiplex(const UavMultiplex *inMultiplex) {
73    multiplex=(UavMultiplex*)inMultiplex;
74    getFrameworkManager()->AddDeviceToLog(multiplex);
75}
76
77void Uav::SetVerticalCamera(const Camera* inVerticalCamera) {
78    verticalCamera=(Camera*)inVerticalCamera;
79}
80/*
81void Uav::SetupVRPNSerial(SerialPort *vrpn_port,string name,int VRPNSerialObjectId) {
82    vrpnclient=new VrpnClient(getFrameworkManager(),"vrpn",vrpn_port,10000,80);
83    uav_vrpn=new MetaVrpnObject(vrpnclient,name,VRPNSerialObjectId);
84
85    getFrameworkManager()->AddDeviceToLog(uav_vrpn);
86}
87*/
88void Uav::SetupVRPNAutoIP(string name) {
89    SetupVRPN("192.168.147.197:3883",name);
90}
91
92void Uav::SetupVRPN(string optitrack_address,string name) {
93    vrpnclient=new VrpnClient(getFrameworkManager(),"vrpn",optitrack_address,10000,80);
94    uav_vrpn=new MetaVrpnObject(vrpnclient,name);
95
96    getFrameworkManager()->AddDeviceToLog(uav_vrpn);
97
98    GetAhrs()->YawPlot()->AddCurve(uav_vrpn->State()->Element(2),DataPlot::Green);
99    //GetAhrs()->RollPlot()->AddCurve(uav_vrpn->State()->Element(0),DataPlot::Green);
100    //GetAhrs()->PitchPlot()->AddCurve(uav_vrpn->State()->Element(1),DataPlot::Green);
101}
102
103void Uav::StartSensors(void)  {
104    if(vrpnclient!=NULL) {
105        vrpnclient->Start();
106    }
107}
108void Uav::UseDefaultPlot(void) {
109    multiplex->UseDefaultPlot();
110
111    if(bldc->HasSpeedMeasurement()) {
112        Tab* plot_tab=new Tab(multiplex->GetTabWidget(),"Speeds");
113        DataPlot1D* plots[4];
114        plots[0]=new DataPlot1D(plot_tab->NewRow(),"front left",0,7000);
115        plots[1]=new DataPlot1D(plot_tab->LastRowLastCol(),"front right",0,7000);
116        plots[2]=new DataPlot1D(plot_tab->NewRow(),"rear left",0,7000);
117        plots[3]=new DataPlot1D(plot_tab->LastRowLastCol(),"rear right",0,7000);
118
119        if(bldc->MotorsCount()==8) {
120            for(int i=0;i<4;i++) plots[i]->AddCurve(bldc->Output()->Element(multiplex->MultiplexValue(i),0),DataPlot::Red,"top");
121            for(int i=0;i<4;i++) plots[i]->AddCurve(bldc->Output()->Element(multiplex->MultiplexValue(i+4),0),DataPlot::Blue,"bottom");
122        } else {
123            for(int i=0;i<4;i++) plots[i]->AddCurve(bldc->Output()->Element(multiplex->MultiplexValue(i),0));
124        }
125    }
126
127    if(bldc->HasCurrentMeasurement()) {
128        Tab* plot_tab=new Tab(multiplex->GetTabWidget(),"Currents");
129        DataPlot1D* plots[4];
130        plots[0]=new DataPlot1D(plot_tab->NewRow(),"front left",0,10);
131        plots[1]=new DataPlot1D(plot_tab->LastRowLastCol(),"front right",0,10);
132        plots[2]=new DataPlot1D(plot_tab->NewRow(),"rear left",0,10);
133        plots[3]=new DataPlot1D(plot_tab->LastRowLastCol(),"rear right",0,10);
134
135        if(bldc->MotorsCount()==8) {
136            for(int i=0;i<4;i++) plots[i]->AddCurve(bldc->Output()->Element(multiplex->MultiplexValue(i),1),DataPlot::Red,"top");
137            for(int i=0;i<4;i++) plots[i]->AddCurve(bldc->Output()->Element(multiplex->MultiplexValue(i+4),1),DataPlot::Blue,"bottom");
138        } else {
139            for(int i=0;i<4;i++) plots[i]->AddCurve(bldc->Output()->Element(multiplex->MultiplexValue(i),1));
140        }
141    }
142
143    meta_us->UseDefaultPlot();
144    ahrs->UseDefaultPlot();
145}
146
147UavMultiplex* Uav::GetUavMultiplex(void) const {
148    return multiplex;
149}
150
151Bldc* Uav::GetBldc(void) const {
152    return bldc;
153}
154
155Ahrs* Uav::GetAhrs(void) const {
156    return ahrs;
157}
158
159Imu* Uav::GetImu(void) const {
160    return imu;
161}
162
163MetaUsRangeFinder* Uav::GetMetaUsRangeFinder(void) const {
164    return meta_us;
165}
166
167UsRangeFinder* Uav::GetUsRangeFinder(void) const {
168    return us;
169}
170
171BatteryMonitor* Uav::GetBatteryMonitor(void) const {
172    return battery;
173}
174
175VrpnClient* Uav::GetVrpnClient(void) const {
176    if(vrpnclient==NULL) Err("vrpn is not setup, call SetupVRPN before\n");
177    return vrpnclient;
178}
179
180MetaVrpnObject* Uav::GetVrpnObject(void) const {
181    return uav_vrpn;
182}
183
184Camera* Uav::GetVerticalCamera(void) const {
185    return verticalCamera;
186}
187
188} // end namespace meta
189} // end namespace flair
Note: See TracBrowser for help on using the repository browser.