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

Last change on this file since 7 was 7, checked in by Sanahuja Guillaume, 5 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.