source: flair-src/branches/mavlink/lib/FlairMeta/src/Uav.cpp@ 98

Last change on this file since 98 was 15, checked in by Bayard Gildas, 6 years ago

sources reformatted with flair-format-dir script

File size: 5.7 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 {
41namespace meta {
42
43Uav::Uav(FrameworkManager *parent, string name, UavMultiplex *multiplex)
44 : Object(parent, name) {
45 vrpnclient = NULL;
46 uav_vrpn = NULL;
47 verticalCamera = NULL;
48 this->multiplex = multiplex;
49}
50
51Uav::~Uav() {}
52
53void Uav::SetUsRangeFinder(const UsRangeFinder *inUs) {
54 us = (UsRangeFinder *)inUs;
55 meta_us = new MetaUsRangeFinder(us);
56 getFrameworkManager()->AddDeviceToLog(us);
57}
58
59void Uav::SetAhrs(const Ahrs *inAhrs) {
60 ahrs = (Ahrs *)inAhrs;
61 imu = (Imu *)ahrs->GetImu();
62 getFrameworkManager()->AddDeviceToLog(imu);
63}
64
65void Uav::SetBldc(const Bldc *inBldc) { bldc = (Bldc *)inBldc; }
66
67void Uav::SetBatteryMonitor(const BatteryMonitor *inBattery) {
68 battery = (BatteryMonitor *)inBattery;
69}
70
71void Uav::SetMultiplex(const UavMultiplex *inMultiplex) {
72 multiplex = (UavMultiplex *)inMultiplex;
73 getFrameworkManager()->AddDeviceToLog(multiplex);
74}
75
76void Uav::SetVerticalCamera(const Camera *inVerticalCamera) {
77 verticalCamera = (Camera *)inVerticalCamera;
78}
79/*
80void Uav::SetupVRPNSerial(SerialPort *vrpn_port,string name,int
81VRPNSerialObjectId) {
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,
94 10000, 80);
95 uav_vrpn = new MetaVrpnObject(vrpnclient, name);
96
97 getFrameworkManager()->AddDeviceToLog(uav_vrpn);
98
99 GetAhrs()->YawPlot()->AddCurve(uav_vrpn->State()->Element(2),
100 DataPlot::Green);
101 // GetAhrs()->RollPlot()->AddCurve(uav_vrpn->State()->Element(0),DataPlot::Green);
102 // GetAhrs()->PitchPlot()->AddCurve(uav_vrpn->State()->Element(1),DataPlot::Green);
103}
104
105void Uav::StartSensors(void) {
106 if (vrpnclient != NULL) {
107 vrpnclient->Start();
108 }
109}
110void Uav::UseDefaultPlot(void) {
111 multiplex->UseDefaultPlot();
112
113 if (bldc->HasSpeedMeasurement()) {
114 Tab *plot_tab = new Tab(multiplex->GetTabWidget(), "Speeds");
115 DataPlot1D *plots[4];
116 plots[0] = new DataPlot1D(plot_tab->NewRow(), "front left", 0, 7000);
117 plots[1] =
118 new DataPlot1D(plot_tab->LastRowLastCol(), "front right", 0, 7000);
119 plots[2] = new DataPlot1D(plot_tab->NewRow(), "rear left", 0, 7000);
120 plots[3] =
121 new DataPlot1D(plot_tab->LastRowLastCol(), "rear right", 0, 7000);
122
123 if (bldc->MotorsCount() == 8) {
124 for (int i = 0; i < 4; i++)
125 plots[i]->AddCurve(
126 bldc->Output()->Element(multiplex->MultiplexValue(i), 0),
127 DataPlot::Red, "top");
128 for (int i = 0; i < 4; i++)
129 plots[i]->AddCurve(
130 bldc->Output()->Element(multiplex->MultiplexValue(i + 4), 0),
131 DataPlot::Blue, "bottom");
132 } else {
133 for (int i = 0; i < 4; i++)
134 plots[i]->AddCurve(
135 bldc->Output()->Element(multiplex->MultiplexValue(i), 0));
136 }
137 }
138
139 if (bldc->HasCurrentMeasurement()) {
140 Tab *plot_tab = new Tab(multiplex->GetTabWidget(), "Currents");
141 DataPlot1D *plots[4];
142 plots[0] = new DataPlot1D(plot_tab->NewRow(), "front left", 0, 10);
143 plots[1] = new DataPlot1D(plot_tab->LastRowLastCol(), "front right", 0, 10);
144 plots[2] = new DataPlot1D(plot_tab->NewRow(), "rear left", 0, 10);
145 plots[3] = new DataPlot1D(plot_tab->LastRowLastCol(), "rear right", 0, 10);
146
147 if (bldc->MotorsCount() == 8) {
148 for (int i = 0; i < 4; i++)
149 plots[i]->AddCurve(
150 bldc->Output()->Element(multiplex->MultiplexValue(i), 1),
151 DataPlot::Red, "top");
152 for (int i = 0; i < 4; i++)
153 plots[i]->AddCurve(
154 bldc->Output()->Element(multiplex->MultiplexValue(i + 4), 1),
155 DataPlot::Blue, "bottom");
156 } else {
157 for (int i = 0; i < 4; i++)
158 plots[i]->AddCurve(
159 bldc->Output()->Element(multiplex->MultiplexValue(i), 1));
160 }
161 }
162
163 meta_us->UseDefaultPlot();
164 ahrs->UseDefaultPlot();
165}
166
167UavMultiplex *Uav::GetUavMultiplex(void) const { return multiplex; }
168
169Bldc *Uav::GetBldc(void) const { return bldc; }
170
171Ahrs *Uav::GetAhrs(void) const { return ahrs; }
172
173Imu *Uav::GetImu(void) const { return imu; }
174
175MetaUsRangeFinder *Uav::GetMetaUsRangeFinder(void) const { return meta_us; }
176
177UsRangeFinder *Uav::GetUsRangeFinder(void) const { return us; }
178
179BatteryMonitor *Uav::GetBatteryMonitor(void) const { return battery; }
180
181VrpnClient *Uav::GetVrpnClient(void) const {
182 if (vrpnclient == NULL)
183 Err("vrpn is not setup, call SetupVRPN before\n");
184 return vrpnclient;
185}
186
187MetaVrpnObject *Uav::GetVrpnObject(void) const { return uav_vrpn; }
188
189Camera *Uav::GetVerticalCamera(void) const { return verticalCamera; }
190
191} // end namespace meta
192} // end namespace flair
Note: See TracBrowser for help on using the repository browser.