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
Line 
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}
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.