// %flair:license{ // This file is part of the Flair framework distributed under the // CECILL-C License, Version 1.0. // %flair:license} // created: 2014/06/10 // filename: Uav.cpp // // author: Guillaume Sanahuja // Copyright Heudiasyc UMR UTC/CNRS 7253 // // version: $Id: $ // // purpose: Base class to construct sensors/actuators depending on uav type // // /*********************************************************************/ #include "Uav.h" #include #include #include #include #include #include #include #include #include #include #include #include "MetaUsRangeFinder.h" #include "MetaVrpnObject.h" using std::string; using namespace flair::core; using namespace flair::gui; using namespace flair::filter; using namespace flair::sensor; using namespace flair::actuator; namespace flair { namespace meta { Uav::Uav(FrameworkManager* parent,string name,UavMultiplex *multiplex): Object(parent,name) { vrpnclient=NULL; uav_vrpn=NULL; verticalCamera=NULL; this->multiplex=multiplex; } Uav::~Uav() { } void Uav::SetUsRangeFinder(const UsRangeFinder *inUs) { us=(UsRangeFinder*)inUs; meta_us=new MetaUsRangeFinder(us); getFrameworkManager()->AddDeviceToLog(us); } void Uav::SetAhrs(const Ahrs *inAhrs) { ahrs=(Ahrs*)inAhrs; imu=(Imu*)ahrs->GetImu(); getFrameworkManager()->AddDeviceToLog(imu); } void Uav::SetBldc(const Bldc* inBldc) { bldc=(Bldc*)inBldc; } void Uav::SetBatteryMonitor(const BatteryMonitor* inBattery) { battery=(BatteryMonitor*)inBattery; } void Uav::SetMultiplex(const UavMultiplex *inMultiplex) { multiplex=(UavMultiplex*)inMultiplex; getFrameworkManager()->AddDeviceToLog(multiplex); } void Uav::SetVerticalCamera(const Camera* inVerticalCamera) { verticalCamera=(Camera*)inVerticalCamera; } /* void Uav::SetupVRPNSerial(SerialPort *vrpn_port,string name,int VRPNSerialObjectId) { vrpnclient=new VrpnClient(getFrameworkManager(),"vrpn",vrpn_port,10000,80); uav_vrpn=new MetaVrpnObject(vrpnclient,name,VRPNSerialObjectId); getFrameworkManager()->AddDeviceToLog(uav_vrpn); } */ void Uav::SetupVRPNAutoIP(string name) { SetupVRPN("192.168.147.197:3883",name); } void Uav::SetupVRPN(string optitrack_address,string name) { vrpnclient=new VrpnClient(getFrameworkManager(),"vrpn",optitrack_address,10000,80); uav_vrpn=new MetaVrpnObject(vrpnclient,name); getFrameworkManager()->AddDeviceToLog(uav_vrpn); GetAhrs()->YawPlot()->AddCurve(uav_vrpn->State()->Element(2),DataPlot::Green); //GetAhrs()->RollPlot()->AddCurve(uav_vrpn->State()->Element(0),DataPlot::Green); //GetAhrs()->PitchPlot()->AddCurve(uav_vrpn->State()->Element(1),DataPlot::Green); } void Uav::StartSensors(void) { if(vrpnclient!=NULL) { vrpnclient->Start(); } } void Uav::UseDefaultPlot(void) { multiplex->UseDefaultPlot(); if(bldc->HasSpeedMeasurement()) { Tab* plot_tab=new Tab(multiplex->GetTabWidget(),"Speeds"); DataPlot1D* plots[4]; plots[0]=new DataPlot1D(plot_tab->NewRow(),"front left",0,7000); plots[1]=new DataPlot1D(plot_tab->LastRowLastCol(),"front right",0,7000); plots[2]=new DataPlot1D(plot_tab->NewRow(),"rear left",0,7000); plots[3]=new DataPlot1D(plot_tab->LastRowLastCol(),"rear right",0,7000); if(bldc->MotorsCount()==8) { for(int i=0;i<4;i++) plots[i]->AddCurve(bldc->Output()->Element(multiplex->MultiplexValue(i),0),DataPlot::Red,"top"); for(int i=0;i<4;i++) plots[i]->AddCurve(bldc->Output()->Element(multiplex->MultiplexValue(i+4),0),DataPlot::Blue,"bottom"); } else { for(int i=0;i<4;i++) plots[i]->AddCurve(bldc->Output()->Element(multiplex->MultiplexValue(i),0)); } } if(bldc->HasCurrentMeasurement()) { Tab* plot_tab=new Tab(multiplex->GetTabWidget(),"Currents"); DataPlot1D* plots[4]; plots[0]=new DataPlot1D(plot_tab->NewRow(),"front left",0,10); plots[1]=new DataPlot1D(plot_tab->LastRowLastCol(),"front right",0,10); plots[2]=new DataPlot1D(plot_tab->NewRow(),"rear left",0,10); plots[3]=new DataPlot1D(plot_tab->LastRowLastCol(),"rear right",0,10); if(bldc->MotorsCount()==8) { for(int i=0;i<4;i++) plots[i]->AddCurve(bldc->Output()->Element(multiplex->MultiplexValue(i),1),DataPlot::Red,"top"); for(int i=0;i<4;i++) plots[i]->AddCurve(bldc->Output()->Element(multiplex->MultiplexValue(i+4),1),DataPlot::Blue,"bottom"); } else { for(int i=0;i<4;i++) plots[i]->AddCurve(bldc->Output()->Element(multiplex->MultiplexValue(i),1)); } } meta_us->UseDefaultPlot(); ahrs->UseDefaultPlot(); } UavMultiplex* Uav::GetUavMultiplex(void) const { return multiplex; } Bldc* Uav::GetBldc(void) const { return bldc; } Ahrs* Uav::GetAhrs(void) const { return ahrs; } Imu* Uav::GetImu(void) const { return imu; } MetaUsRangeFinder* Uav::GetMetaUsRangeFinder(void) const { return meta_us; } UsRangeFinder* Uav::GetUsRangeFinder(void) const { return us; } BatteryMonitor* Uav::GetBatteryMonitor(void) const { return battery; } VrpnClient* Uav::GetVrpnClient(void) const { if(vrpnclient==NULL) Err("vrpn is not setup, call SetupVRPN before\n"); return vrpnclient; } MetaVrpnObject* Uav::GetVrpnObject(void) const { return uav_vrpn; } Camera* Uav::GetVerticalCamera(void) const { return verticalCamera; } } // end namespace meta } // end namespace flair