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

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

filter and meta

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