[10] | 1 | // %flair:license{
|
---|
[15] | 2 | // This file is part of the Flair framework distributed under the
|
---|
| 3 | // CECILL-C License, Version 1.0.
|
---|
[10] | 4 | // %flair:license}
|
---|
[7] | 5 | // created: 2016/02/08
|
---|
| 6 | // filename: SimuX4.cpp
|
---|
| 7 | //
|
---|
| 8 | // author: Guillaume Sanahuja
|
---|
| 9 | // Copyright Heudiasyc UMR UTC/CNRS 7253
|
---|
| 10 | //
|
---|
| 11 | // version: $Id: $
|
---|
| 12 | //
|
---|
[22] | 13 | // purpose: Class defining a simulation x4 uav
|
---|
[7] | 14 | //
|
---|
| 15 | //
|
---|
| 16 | /*********************************************************************/
|
---|
| 17 |
|
---|
| 18 | #include "SimuX4.h"
|
---|
| 19 | #include <FrameworkManager.h>
|
---|
| 20 | #include <X4X8Multiplex.h>
|
---|
| 21 | #include <SimuImu.h>
|
---|
| 22 | #include <SimuAhrs.h>
|
---|
| 23 | #include <SimuBldc.h>
|
---|
| 24 | #include <SimuUs.h>
|
---|
| 25 | #include <SimuCamera.h>
|
---|
[245] | 26 | #include <SimuPressureSensor.h>
|
---|
[7] | 27 | #include <BatteryMonitor.h>
|
---|
[185] | 28 | #include <SimuGps.h>
|
---|
[7] | 29 | #include <Tab.h>
|
---|
[157] | 30 | #include <FindArgument.h>
|
---|
[7] | 31 |
|
---|
| 32 | using std::string;
|
---|
| 33 | using namespace flair::core;
|
---|
| 34 | using namespace flair::gui;
|
---|
| 35 | using namespace flair::sensor;
|
---|
| 36 | using namespace flair::filter;
|
---|
| 37 | using namespace flair::actuator;
|
---|
| 38 |
|
---|
[15] | 39 | namespace flair {
|
---|
| 40 | namespace meta {
|
---|
[7] | 41 |
|
---|
[158] | 42 | SimuX4::SimuX4(string name, uint32_t simu_id,string options,
|
---|
[15] | 43 | filter::UavMultiplex *multiplex)
|
---|
[122] | 44 | : Uav(name, multiplex) {
|
---|
[7] | 45 |
|
---|
[15] | 46 | if (multiplex == NULL)
|
---|
[137] | 47 | SetMultiplex(new X4X8Multiplex("motors", X4X8Multiplex::X4));
|
---|
[7] | 48 |
|
---|
[15] | 49 | SetBldc(new SimuBldc(GetUavMultiplex(), GetUavMultiplex()->GetLayout(),
|
---|
[158] | 50 | "motors", GetUavMultiplex()->MotorsCount(), simu_id,0));
|
---|
| 51 | SetUsRangeFinder(new SimuUs("us", simu_id,0, 60));
|
---|
[186] | 52 | SetAhrs(new SimuAhrs("ahrs", simu_id, 0,70));
|
---|
[122] | 53 | Tab *bat_tab = new Tab(getFrameworkManager()->GetTabWidget(), "battery");
|
---|
[15] | 54 | SetBatteryMonitor(new BatteryMonitor(bat_tab->NewRow(), "battery"));
|
---|
| 55 | GetBatteryMonitor()->SetBatteryValue(12);
|
---|
[157] | 56 |
|
---|
| 57 | uint16_t camvWidth=320,camvHeight=240;
|
---|
[160] | 58 | ReadCameraResolutionOption(options,"camv",camvWidth,camvHeight);
|
---|
| 59 | Info("using vertical camera resolution: %ix%i\n",camvWidth, camvHeight);
|
---|
| 60 | SetVerticalCamera(new SimuCamera("simu_cam_v", camvWidth, camvHeight, 3, simu_id,0, 10));
|
---|
[157] | 61 |
|
---|
[160] | 62 | uint16_t camhWidth=320,camhHeight=240;
|
---|
| 63 | ReadCameraResolutionOption(options,"camh",camhWidth,camhHeight);
|
---|
| 64 | Info("using horizontal camera resolution: %ix%i\n",camhWidth, camhHeight);
|
---|
| 65 | SetHorizontalCamera(new SimuCamera("simu_cam_h", camhWidth, camhHeight, 3, simu_id,1, 10));
|
---|
[185] | 66 |
|
---|
| 67 | string useGps=FindArgument(options,"use_gps=",false);
|
---|
| 68 | if(useGps=="true") {
|
---|
| 69 | SetGps(new SimuGps("gps", (NmeaGps::NMEAFlags_t)(NmeaGps::GGA | NmeaGps::VTG), 0,0, 40));
|
---|
[245] | 70 | }
|
---|
| 71 |
|
---|
| 72 | string usePressureSensor=FindArgument(options,"use_pressure_sensor=",false);
|
---|
| 73 | if(usePressureSensor=="true") {
|
---|
| 74 | SetPressureSensor(new SimuPressureSensor("pressuresensor", 0,0, 10));
|
---|
[185] | 75 | }
|
---|
[7] | 76 | }
|
---|
| 77 |
|
---|
[15] | 78 | SimuX4::~SimuX4() {}
|
---|
[7] | 79 |
|
---|
| 80 | void SimuX4::StartSensors(void) {
|
---|
[157] | 81 | ((SimuImu *)(GetAhrs()->GetImu()))->Start();
|
---|
[15] | 82 | ((SimuUs *)GetUsRangeFinder())->Start();
|
---|
| 83 | ((SimuCamera *)GetVerticalCamera())->Start();
|
---|
[206] | 84 | //((SimuCamera *)GetHorizontalCamera())->Start();
|
---|
[186] | 85 | if(GetGps()) ((SimuGps *)GetGps())->Start();
|
---|
[245] | 86 | if(GetPressureSensor()) ((SimuPressureSensor *)GetPressureSensor())->Start();
|
---|
[7] | 87 | }
|
---|
| 88 |
|
---|
[160] | 89 | void SimuX4::ReadCameraResolutionOption(string options,string cameraName,uint16_t &camWidth,uint16_t &camHeight) const {
|
---|
| 90 | string camOpts=FindArgument(options,cameraName +"=",false);
|
---|
| 91 | if(camOpts!="") {
|
---|
| 92 | size_t position=camOpts.find("x");
|
---|
| 93 | if(position!=std::string::npos) {
|
---|
| 94 | camWidth=std::stoi(camOpts.substr(0,position));
|
---|
| 95 | camHeight=std::stoi(camOpts.substr(position+1,std::string::npos));
|
---|
| 96 | } else {
|
---|
| 97 | Warn("bad camera resolution parameter (%s) should be WIDTHxHEIGHT format\n",camOpts.c_str());
|
---|
| 98 | }
|
---|
| 99 | }
|
---|
| 100 | }
|
---|
[157] | 101 |
|
---|
[7] | 102 | } // end namespace meta
|
---|
| 103 | } // end namespace flair
|
---|