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

Last change on this file since 121 was 121, checked in by Sanahuja Guillaume, 5 years ago

modifs camera

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