Changeset 55 in flair-src for trunk


Ignore:
Timestamp:
Jul 28, 2016, 5:55:31 PM (8 years ago)
Author:
Sanahuja Guillaume
Message:

simu gps

Location:
trunk/lib
Files:
13 edited

Legend:

Unmodified
Added
Removed

  • trunk/lib/FlairCore/src/FrameworkManager.cpp

    r45 r55  
    133133}
    134134
    135 void FrameworkManager::SetupConnection(string address, uint16_t port,
     135void FrameworkManager::SetupConnection(string address, uint16_t port,Time watchdogTimeout,
    136136                                       size_t rcv_buf_size) {
    137137  pimpl_->SetupConnection(address, port, rcv_buf_size);
    … …  
    143143
    144144gui::TabWidget *FrameworkManager::GetTabWidget(void) const {
     145  if(pimpl_->tabwidget==NULL) Err("SetupUserInterface must be called before\n");
    145146  return pimpl_->tabwidget;
    146147}

  • trunk/lib/FlairCore/src/FrameworkManager.h

    r15 r55  
    1919
    2020namespace flair {
    21 namespace gui {
    22 class TabWidget;
    23 class SendData;
    24 }
     21  namespace gui {
     22    class TabWidget;
     23    class SendData;
     24  }
    2525}
    2626namespace flair {
    … …  
    6969  *
    7070  * Call this method just after the constructor of this class. If this method is
    71   *not called, the program will run headless.
     71  * not called, the program will run headless.
     72  * If this method is called, SetupUserInterface must also be called after this.
    7273  *
    7374  * \param address address of ground station
    7475  * \param port port of ground station
     76  * \param watchdogTimeout watchdog timeout for the connection, passing TIME_INFINITE will disable the watchdog
    7577  * \param rcv_buf_size receive buffer size
    7678  */
    77   void SetupConnection(std::string address, uint16_t port,
     79  void SetupConnection(std::string address, uint16_t port,Time watchdogTimeout=(Time)1000000000,
    7880                       size_t rcv_buf_size = 10000);
    7981
    … …  
    8284  *
    8385  * If this method is called after SetupConnection, Widgets will be displayed in
    84   *the ground station.
    85   * Otherwise, it will run headless, but default values of Widgets will be taken
    86   *from the xml file.
     86  * the ground station.
     87  * If this method is called and SetupConnection was not called, it will run headless but default values of Widgets will be taken
     88  * from the xml file.
     89  * If this method is not called, Widgets will not be available. Constructing an object based on Widget class will fail.
    8790  *
    8891  * \param xml_file xml file for default values of Widgets

  • trunk/lib/FlairCore/src/FrameworkManager_impl.cpp

    r51 r55  
    103103  gcs_watchdog = NULL;
    104104  _this = this;
     105  tabwidget=NULL;
    105106
    106107  // Avoids memory swapping for this program
    … …  
    193194}
    194195
    195 void FrameworkManager_impl::SetupConnection(string address, uint16_t port,
     196void FrameworkManager_impl::SetupConnection(string address, uint16_t port,Time watchdogTimeout,
    196197                                            size_t rcv_buf_size) {
    197198  UDT::startup();
    … …  
    804805        if (tmp.running == true) // start logging
    805806        {
    806           filename =
    807               caller->log_path + "/" + caller->FileName(tmp.device) + ".dbt";
    808           printf("Creating log file %s (log size %i)\n", filename.c_str(),
    809                  (int)tmp.size);
     807          filename = caller->log_path + "/" + caller->FileName(tmp.device) + ".dbt";
     808          printf("Creating log file %s (log size %i)\n", filename.c_str(), (int)tmp.size);
    810809          tmp.dbtFile = inithdFile((char *)filename.c_str(), UAV, tmp.size);
    811810          logs.push_back(tmp);

  • trunk/lib/FlairCore/src/unexported/FrameworkManager_impl.h

    r15 r55  
    4343  FrameworkManager_impl(flair::core::FrameworkManager *self, std::string name);
    4444  ~FrameworkManager_impl();
    45   void SetupConnection(std::string address, uint16_t port,
     45  void SetupConnection(std::string address, uint16_t port,flair::core::Time watchdogTimeout,
    4646                       size_t rcv_buf_size = 10000);
    4747  void SetupUserInterface(std::string xml_file);

  • trunk/lib/FlairSensorActuator/src/Imu.cpp

    r15 r55  
    5959
    6060void Imu::GetDatas(ImuData **outImuData) const { *outImuData = imuData; }
     61
    6162void Imu::UpdateImu() {
    6263  if (rotation == NULL) {

  • trunk/lib/FlairSensorActuator/src/NmeaGps.cpp

    r51 r55  
    4747  nmea_zero_INFO(&info);
    4848  nmea_parser_init(&parser);
    49   alt_ref = 0;
     49  altRef = 0;
    5050
    5151  if ((NMEAFlags & GGA) == 0) {
    … …  
    6363
    6464  // station sol
    65   main_tab = new Tab(parent->GetTabWidget(), name);
    66   tab = new TabWidget(main_tab->NewRow(), name);
    67   sensor_tab = new Tab(tab, "Reglages");
    68   GroupBox *reglages_groupbox = new GroupBox(sensor_tab->NewRow(), name);
    69   button_ref = new PushButton(reglages_groupbox->NewRow(), "set ref");
    70   nb_sat_label = new Label(reglages_groupbox->NewRow(), "nb_sat");
    71   fix_label = new Label(reglages_groupbox->LastRowLastCol(), "fix");
     65  mainTab = new Tab(parent->GetTabWidget(), name);
     66  tab = new TabWidget(mainTab->NewRow(), name);
     67  sensorTab = new Tab(tab, "Setup");
     68  setupGroupbox = new GroupBox(sensorTab->NewRow(), name);
     69  buttonRef = new PushButton(setupGroupbox->NewRow(), "set ref");
     70  nbSatLabel = new Label(setupGroupbox->NewRow(), "nb_sat");
     71  fixLabel = new Label(setupGroupbox->LastRowLastCol(), "fix");
    7272
    7373  position = new GeoCoordinate((IODevice *)this, "position", 0, 0, 0);
    7474
    75   take_ref = false;
     75  takeRef = false;
    7676
    7777  gpsData = new GpsData(this);
    7878  AddDataToLog(gpsData);
    7979
    80   nb_sat_label->SetText("nb_sat: %i", gpsData->GetNumberOfSatellites());
    81   fix_label->SetText("fix: %i", gpsData->GetFixQuality());
     80  nbSatLabel->SetText("number of satellies: %i", gpsData->GetNumberOfSatellites());
     81  fixLabel->SetText("fix quality: %i", gpsData->GetFixQuality());
     82}
     83
     84NmeaGps::NmeaGps(const IODevice *parent, std::string name) : IODevice(parent, name) {
     85  gpsData = new GpsData(this);
     86  mainTab = NULL;
     87  tab = NULL;
     88  sensorTab = NULL;
    8289}
    8390
    8491NmeaGps::~NmeaGps() {
    85   nmea_parser_destroy(&parser);
    86   delete main_tab;
    87 }
     92  if (mainTab != NULL) {
     93    nmea_parser_destroy(&parser);
     94    delete mainTab;
     95  }
     96}
     97
     98GroupBox *NmeaGps::GetGroupBox(void) const { return setupGroupbox; }
    8899
    89100const GpsData *NmeaGps::GetDatas(void) const {
    … …  
    96107
    97108void NmeaGps::UseDefaultPlot(void) {
    98   plot_tab = new Tab(tab, "Mesures");
    99 
    100   if ((NMEAFlags & GGA) != 0) {
    101     e_plot = new DataPlot1D(plot_tab->NewRow(), "e", -10, 10);
    102     e_plot->AddCurve(gpsData->Element(GpsData::East));
    103     n_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "n", -10, 10);
    104     n_plot->AddCurve(gpsData->Element(GpsData::North));
    105     u_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "u", -10, 10);
    106     u_plot->AddCurve(gpsData->Element(GpsData::Up));
     109  plotTab = new Tab(tab, "Mesures");
     110
     111  if ((NMEAFlags & GGA) != 0) {
     112    ePlot = new DataPlot1D(plotTab->NewRow(), "e", -10, 10);
     113    ePlot->AddCurve(gpsData->Element(GpsData::East));
     114    nPlot = new DataPlot1D(plotTab->LastRowLastCol(), "n", -10, 10);
     115    nPlot->AddCurve(gpsData->Element(GpsData::North));
     116    uPlot = new DataPlot1D(plotTab->LastRowLastCol(), "u", -10, 10);
     117    uPlot->AddCurve(gpsData->Element(GpsData::Up));
    107118  }
    108119  if ((NMEAFlags & VTG) != 0) {
    109     ve_plot = new DataPlot1D(plot_tab->NewRow(), "ve", -10, 10);
    110     ve_plot->AddCurve(gpsData->Element(GpsData::EastVelocity));
    111     vn_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "vn", -10, 10);
    112     vn_plot->AddCurve(gpsData->Element(GpsData::NorthVelocity));
     120    vePlot = new DataPlot1D(plotTab->NewRow(), "ve", -10, 10);
     121    vePlot->AddCurve(gpsData->Element(GpsData::EastVelocity));
     122    vnPlot = new DataPlot1D(plotTab->LastRowLastCol(), "vn", -10, 10);
     123    vnPlot->AddCurve(gpsData->Element(GpsData::NorthVelocity));
    113124  }
    114125
    … …  
    120131DataPlot1D *NmeaGps::EPlot(void) const {
    121132  if ((NMEAFlags & GGA) != 0) {
    122     return e_plot;
     133    return ePlot;
    123134  } else {
    124135    Err("GGA sentence not requested\n");
    … …  
    129140DataPlot1D *NmeaGps::NPlot(void) const {
    130141  if ((NMEAFlags & GGA) != 0) {
    131     return n_plot;
     142    return nPlot;
    132143  } else {
    133144    Err("GGA sentence not requested\n");
    … …  
    138149DataPlot1D *NmeaGps::UPlot(void) const {
    139150  if ((NMEAFlags & GGA) != 0) {
    140     return u_plot;
     151    return uPlot;
    141152  } else {
    142153    Err("GGA sentence not requested\n");
    … …  
    147158DataPlot1D *NmeaGps::VEPlot(void) const {
    148159  if ((NMEAFlags & VTG) != 0) {
    149     return ve_plot;
     160    return vePlot;
    150161  } else {
    151162    Err("GGA sentence not requested\n");
    … …  
    156167DataPlot1D *NmeaGps::VNPlot(void) const {
    157168  if ((NMEAFlags & VTG) != 0) {
    158     return vn_plot;
    159   } else {
    160     Err("GGA sentence not requested\n");
    161     return NULL;
    162   }
    163 }
    164 
    165 Layout *NmeaGps::GetLayout(void) const { return sensor_tab; }
    166 
    167 Tab *NmeaGps::GetPlotTab(void) const { return plot_tab; }
     169    return vnPlot;
     170  } else {
     171    Err("GGA sentence not requested\n");
     172    return NULL;
     173  }
     174}
     175
     176Layout *NmeaGps::GetLayout(void) const { return sensorTab; }
     177
     178Tab *NmeaGps::GetPlotTab(void) const { return plotTab; }
    168179
    169180TabWidget *NmeaGps::GetTab(void) const { return tab; }
    170181
    171 void NmeaGps::SetRef(void) { take_ref = true; }
     182void NmeaGps::SetRef(void) { takeRef = true; }
    172183
    173184void NmeaGps::GetEnu(Vector3D *point) {
    … …  
    197208    if (gpsData->GetFixQuality() != (GpsData::FixQuality_t)pack.sig) {
    198209      gpsData->SetFixQuality((GpsData::FixQuality_t)pack.sig);
    199       fix_label->SetText("fix: %i", pack.sig);
     210      fixLabel->SetText("fix: %i", pack.sig);
    200211    }
    201212    if (gpsData->GetNumberOfSatellites() != pack.satinuse) {
    202213      gpsData->SetNumberOfSatellites(pack.satinuse) ;
    203       nb_sat_label->SetText("nb_sat: %i", pack.satinuse);
     214      nbSatLabel->SetText("nb_sat: %i", pack.satinuse);
    204215    }
    205216    gpsData->ReleaseMutex();
    … …  
    209220                             info.elv);
    210221
    211     if ((info.sig == 2 && alt_ref == 0) || button_ref->Clicked() == true ||
    212         take_ref == true) {
     222    if ((info.sig == 2 && altRef == 0) || buttonRef->Clicked() == true ||
     223        takeRef == true) {
    213224      Printf("prise pos ref\n");
    214       lat_ref = pos.lat;
    215       long_ref = pos.lon;
    216       alt_ref = info.elv;
    217       take_ref = false;
     225      latRef = pos.lat;
     226      longRef = pos.lon;
     227      altRef = info.elv;
     228      takeRef = false;
    218229    }
    219230    // if(alt_ref!=0)
    … …  
    222233      double e, n, u;
    223234      Geographique_2_ECEF(pos.lon, pos.lat, info.elv, x, y, z);
    224       ECEF_2_ENU(x, y, z, e, n, u, long_ref, lat_ref, alt_ref);
     235      ECEF_2_ENU(x, y, z, e, n, u, longRef, latRef, altRef);
    225236      // Printf("lon:%f lat:%f elv:%f\n",pos.lon,pos.lat,info.elv);
    226237

  • trunk/lib/FlairSensorActuator/src/NmeaGps.h

    r51 r55  
    3232    class Map;
    3333    class Label;
     34    class GroupBox;
    3435  }
    3536}
    … …  
    6768
    6869  /*!
     70  * \brief Constructor
     71  *
     72  * Construct a NmeaGps. \n
     73  * This contructor must only be called for a simulated device.
     74  *
     75  * \param parent parent
     76  * \param name name
     77  */
     78  NmeaGps(const core::IODevice *parent, std::string name);
     79
     80  /*!
    6981  * \brief Destructor
    7082  *
    … …  
    173185  NMEAFlags_t NMEAFlags;
    174186
    175 protected:
     187  /*!
     188  * \brief Get GPS datas
     189  *
     190  * \param gpsData GPS datas
     191  */
     192  void GetDatas(core::GpsData **gpsData) const;
     193
     194  /*!
     195  * \brief Setup GroupBox
     196  *
     197  * \return setup GroupBox
     198  */
     199  gui::GroupBox *GetGroupBox(void) const;
     200
     201private:
     202  gui::Tab *mainTab, *sensorTab;
     203  gui::TabWidget *tab;
     204  gui::GroupBox *setupGroupbox;
     205  gui::PushButton *buttonRef;
     206  gui::DataPlot1D *ePlot;
     207  gui::DataPlot1D *nPlot;
     208  gui::DataPlot1D *uPlot;
     209  gui::DataPlot1D *vePlot;
     210  gui::DataPlot1D *vnPlot;
     211  gui::Tab *plotTab;
     212  gui::Map *map;
     213  gui::Label *nbSatLabel, *fixLabel;
    176214  core::GeoCoordinate *position;
    177 
    178   /*!
    179   * \brief Get GPS datas
    180   *
    181   * \param gpsData GPS datas
    182   */
    183   void GetDatas(core::GpsData **gpsData) const;
    184 
    185 private:
    186   /*!
    187   * \brief Update using provided datas
    188   *
    189   * Reimplemented from IODevice.
    190   *
    191   * \param data data from the parent to process
    192   */
    193   void UpdateFrom(const core::io_data *data){};
    194 
    195   gui::Tab *main_tab, *sensor_tab;
    196   gui::TabWidget *tab;
    197   gui::PushButton *button_ref;
    198   gui::DataPlot1D *e_plot;
    199   gui::DataPlot1D *n_plot;
    200   gui::DataPlot1D *u_plot;
    201   gui::DataPlot1D *ve_plot;
    202   gui::DataPlot1D *vn_plot;
    203   gui::Tab *plot_tab;
    204   gui::Map *map;
    205   gui::Label *nb_sat_label, *fix_label;
    206   bool take_ref;
     215  bool takeRef;
    207216  nmeaINFO info;
    208217  nmeaPARSER parser;
    209218  nmeaGPGGA pack;
    210219  nmeaPOS pos;
    211   double lat_ref, long_ref, alt_ref;
     220  double latRef, longRef, altRef;
    212221  core::GpsData* gpsData;
    213222};

  • trunk/lib/FlairSensorActuator/src/SimuGps.cpp

    r51 r55  
    1818#include "SimuGps.h"
    1919#include <FrameworkManager.h>
    20 #include <string.h>
    2120#include <GeoCoordinate.h>
    2221#include <GpsData.h>
     22#include <SharedMem.h>
     23#include <SpinBox.h>
     24#include <DoubleSpinBox.h>
     25#include <GroupBox.h>
     26#include <Euler.h>
     27#include <cvmatrix.h>
     28#include <sstream>
     29#include "geodesie.h"
    2330
    2431using std::string;
     32using std::ostringstream;
    2533using namespace flair::core;
     34using namespace flair::gui;
     35using namespace Geodesie;
    2636
    2737namespace flair {
    … …  
    2939
    3040SimuGps::SimuGps(const FrameworkManager *parent, string name,
    31                  NmeaGps::NMEAFlags_t NMEAFlags, uint8_t priority)
    32     : Thread(parent, name, priority), NmeaGps(parent, name, NMEAFlags) {}
     41                 NmeaGps::NMEAFlags_t NMEAFlags, uint32_t deviceId,uint8_t priority)
     42    : NmeaGps(parent, name, NMEAFlags),Thread(parent, name, priority) {
     43
     44  dataRate = new SpinBox(GetGroupBox()->NewRow(), "data rate", " Hz", 1, 500, 1, 200);
     45  latitudeRef = new DoubleSpinBox(GetGroupBox()->NewRow(), "latitude ref", " deg", -90, 90, 1, 6,49.402313);
     46  longitudeRef = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "longitude ref", " deg", -180, 180, 1, 6,2.795463);
     47  altitudeRef= new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "altitude ref", " m", 0, 6000, 100, 1,0);
     48  fixQuality = new SpinBox(GetGroupBox()->NewRow(), "fix quality", 1, 8, 1, 1);
     49  numberOfSatellites = new SpinBox(GetGroupBox()->NewRow(), "number of satellites", 1, 15, 1, 5);
     50
     51  ostringstream dev_name;
     52  dev_name << "simu_gps_" << deviceId;
     53  shmem = new SharedMem((Thread *)this, dev_name.str().c_str(),
     54                        sizeof(gps_states_t));
     55}
     56
     57
     58SimuGps::SimuGps(const IODevice *parent, string name, uint32_t deviceId)
     59    : NmeaGps(parent, name), Thread(parent, name, 0) {
     60  dataRate = NULL;
     61
     62  ostringstream dev_name;
     63  dev_name << "simu_gps_" << deviceId;
     64  shmem = new SharedMem((Thread *)this, dev_name.str().c_str(),
     65                        sizeof(gps_states_t));
     66}
    3367
    3468SimuGps::~SimuGps() {
    … …  
    3771}
    3872
     73void SimuGps::UpdateFrom(const io_data *data) {
     74  if (data != NULL) {
     75    cvmatrix *input = (cvmatrix *)data;
     76    gps_states_t state;
     77
     78    input->GetMutex();
     79    //simulator is ned, convert it to enu
     80    //TODO: put simulator in enu?
     81    state.x = input->ValueNoMutex(5, 0);
     82    state.y = input->ValueNoMutex(4, 0);
     83    state.z = -input->ValueNoMutex(6, 0);
     84    input->ReleaseMutex();
     85
     86    shmem->Write((char *)&state, sizeof(gps_states_t));
     87  }
     88}
     89
    3990void SimuGps::Run(void) {
    40   // double lat=0;
     91  gps_states_t state;
    4192  char buf[500];
    4293  nmeaGPGGA gga;
    4394  nmeaGPVTG vtg;
    44   SetPeriodMS(500);
     95  nmeaPOS pos;
     96  nmeaINFO info;
     97
     98  if (dataRate == NULL) {
     99    Thread::Err("not applicable for simulation part.\n");
     100    return;
     101  }
     102
     103  SetPeriodUS((uint32_t)(1000000. / dataRate->Value()));
     104
    45105  WarnUponSwitches(true);
    46 
    47   gga.sig=1;
    48   gga.satinuse=2;
    49   gga.lat=49.402313;
    50   gga.lon=2.795463;
    51106
    52107  vtg.spn=1;
    … …  
    56111    WaitPeriod();
    57112
     113    if (dataRate->ValueChanged() == true) {
     114      SetPeriodUS((uint32_t)(1000000. / dataRate->Value()));
     115    }
     116
     117    shmem->Read((char *)&state, sizeof(gps_states_t));
     118
     119    double x, y, z;
     120    ENU_2_ECEF(state.x, state.y, state.z, x,y,z, Euler::ToRadian(longitudeRef->Value()), Euler::ToRadian(latitudeRef->Value()), altitudeRef->Value());
     121    ECEF_2_Geographique( x, y, z,pos.lon, pos.lat, gga.elv);
     122    nmea_pos2info(&pos,&info);
     123
     124    if(pos.lat>0) {
     125      gga.ns='N';
     126      gga.lat=info.lat;
     127    } else {
     128      gga.ns='S';
     129      gga.lat=-info.lat;
     130    }
     131    if(pos.lon>0) {
     132      gga.ew='E';
     133      gga.lon=info.lon;
     134    } else {
     135      gga.ew='W';
     136      gga.lon=-info.lon;
     137    }
     138
     139    gga.sig=fixQuality->Value();
     140    gga.satinuse=numberOfSatellites->Value();
     141
    58142    nmea_gen_GPGGA(buf,sizeof(buf),&gga);
    59143    parseFrame(buf,sizeof(buf));
    60     position->SetCoordinates(49.402313, 2.795463, 0);
    61     //      lat+=.5;
    62144  }
    63145

  • trunk/lib/FlairSensorActuator/src/SimuGps.h

    r51 r55  
    1818
    1919namespace flair {
    20 namespace core {
    21 class FrameworkManager;
    22 }
     20  namespace core {
     21    class FrameworkManager;
     22    class SharedMem;
     23  }
     24  namespace gui {
     25    class SpinBox;
     26    class DoubleSpinBox;
     27  }
    2328}
    2429
    … …  
    3439  * \brief Constructor
    3540  *
    36   * Construct a Novatel.
     41  * Construct a simulation GPS. Control part.
    3742  *
    3843  * \param parent parent
    3944  * \param name name
    4045  * \param NMEAFlags NMEA sentances to enable
     46  * \param deviceId device id
    4147  * \param priority priority of the Thread
    4248  */
    4349  SimuGps(const core::FrameworkManager *parent, std::string name,
    44           NmeaGps::NMEAFlags_t NMEAFlags, uint8_t priority);
     50          NmeaGps::NMEAFlags_t NMEAFlags, uint32_t deviceId,uint8_t priority);
     51
     52  /*!
     53  * \brief Constructor
     54  *
     55  * Construct a simulation GPS. Simulation part.\n
     56  * The Thread of this class should not be run.
     57  *
     58  * \param parent parent
     59  * \param name name
     60  * \param deviceId device id
     61  */
     62  SimuGps(const core::IODevice *parent, std::string name, uint32_t deviceId);
    4563
    4664  /*!
    … …  
    5876  * \param data data from the parent to process
    5977  */
    60   void UpdateFrom(const core::io_data *data){};
     78  void UpdateFrom(const core::io_data *data);
    6179
    6280  /*!
    … …  
    6785  */
    6886  void Run(void);
     87
     88  typedef struct {
     89    float x;
     90    float y;
     91    float z;
     92  } gps_states_t;
     93
     94  core::SharedMem *shmem;
     95  gui::SpinBox *dataRate,*fixQuality,*numberOfSatellites;
     96  gui::DoubleSpinBox *latitudeRef,*longitudeRef,*altitudeRef;
    6997};
    7098} // end namespace sensor

  • trunk/lib/FlairSensorActuator/src/SimuImu.cpp

    r15 r55  
    3737                 uint8_t priority)
    3838    : Imu(parent, name), Thread(parent, name, priority) {
    39   data_rate =
     39  dataRate =
    4040      new SpinBox(GetGroupBox()->NewRow(), "data rate", " Hz", 1, 500, 1, 200);
    4141  ahrsData = new AhrsData((Imu *)this);
    … …  
    4949SimuImu::SimuImu(const IODevice *parent, string name, uint32_t dev_id)
    5050    : Imu(parent, name), Thread(parent, name, 0) {
    51   data_rate = NULL;
     51  dataRate = NULL;
    5252
    5353  ostringstream dev_name;
    … …  
    8686  GetDatas(&imuData);
    8787
    88   if (data_rate == NULL) {
     88  if (dataRate == NULL) {
    8989    Thread::Err("not applicable for simulation part.\n");
    9090    return;
    9191  }
    9292
    93   SetPeriodUS((uint32_t)(1000000. / data_rate->Value()));
     93  SetPeriodUS((uint32_t)(1000000. / dataRate->Value()));
    9494
    9595  while (!ToBeStopped()) {
    9696    WaitPeriod();
    9797
    98     if (data_rate->ValueChanged() == true) {
    99       SetPeriodUS((uint32_t)(1000000. / data_rate->Value()));
     98    if (dataRate->ValueChanged() == true) {
     99      SetPeriodUS((uint32_t)(1000000. / dataRate->Value()));
    100100    }
    101101

  • trunk/lib/FlairSensorActuator/src/SimuImu.h

    r15 r55  
    1818
    1919namespace flair {
    20 namespace core {
    21 class SharedMem;
    22 class AhrsData;
    23 }
    24 namespace gui {
    25 class SpinBox;
    26 }
     20  namespace core {
     21    class SharedMem;
     22    class AhrsData;
     23  }
     24  namespace gui {
     25    class SpinBox;
     26  }
    2727}
    2828
    … …  
    9393    float wz;
    9494  } imu_states_t;
    95   gui::SpinBox *data_rate;
     95
     96  gui::SpinBox *dataRate;
    9697  core::SharedMem *shmem;
    9798  core::AhrsData *ahrsData;

  • trunk/lib/FlairSensorActuator/src/geodesie.cpp

    r15 r55  
    322322}
    323323
     324//from https://gist.github.com/klucar/1536194
     325void Geodesie::ECEF_2_Geographique(double x, double y, double z,
     326                                   double &longitude, double &latitude, double &he) {
     327
     328  double asq=pow(GRS_a, 2);
     329  double esq=pow(GRS_e, 2);
     330  double b = sqrt( asq * (1-esq) );
     331  double bsq = pow(b,2);
     332  double ep = sqrt( (asq - bsq)/bsq);
     333  double p = sqrt( pow(x,2) + pow(y,2) );
     334  double th = atan2(GRS_a*z, b*p);
     335
     336  longitude = atan2(y,x);
     337  latitude = atan2( (z + pow(ep,2)*b*pow(sin(th),3) ), (p - esq*GRS_a*pow(cos(th),3)) );
     338  double N = GRS_a/( sqrt(1-esq*pow(sin(latitude),2)) );
     339  double alt = p / cos(latitude) - N;
     340
     341  // mod longitude to 0-2pi
     342  if(longitude<0) longitude +=2*M_PI;
     343
     344  // correction for altitude near poles left out.
     345}
     346
    324347////////////////////////////////////////////////////////////////////////
    325348void Geodesie::ECEF_2_ENU(double x, double y, double z, double &e, double &n,
    … …  
    351374  C.Apply(x, y, z, e, n, u);
    352375}
     376
     377void Geodesie::ENU_2_ECEF(double e, double n,double u,
     378                          double &x, double &y, double &z,
     379                          double lon0, double lat0, double he0) {
     380  double slat = std::sin(lat0);//phi
     381  double clat = std::cos(lat0);
     382  double slon = std::sin(lon0);//lambda
     383  double clon = std::cos(lon0);
     384
     385  Geodesie::Matrice C;
     386  C.c0_l0 = -slon;
     387  C.c1_l0 = -clon*slat;
     388  C.c2_l0 = clon*clat;
     389
     390  C.c0_l1 = clon;
     391  C.c1_l1 = -slon * slat;
     392  C.c2_l1 = slon*clat;
     393
     394  C.c0_l2 = 0;
     395  C.c1_l2 = clat;
     396  C.c2_l2 = slat;
     397
     398  C.Apply(e, n, u,x,y,z);
     399
     400  double x0, y0, z0;
     401  Geographique_2_ECEF(lon0, lat0, he0, x0, y0, z0);
     402
     403  x += x0;
     404  y += y0;
     405  z += z0;
     406}

  • trunk/lib/FlairSensorActuator/src/unexported/geodesie.h

    r16 r55  
    101101void Geographique_2_ECEF(double longitude, double latitude, double he,
    102102                         double &x, double &y, double &z);
    103 /** Convert from ECEF two ENU.
     103
     104/** Convert from ECEF to geographique.
     105 */
     106void ECEF_2_Geographique(double x, double y, double z,
     107                          double &longitude, double &latitude, double &he);
     108
     109/** Convert from ECEF to ENU.
    104110 * @param[in] lon0 Longitude of the origin in radian.
    105111 * @param[in] lat0 Latitude of the origin in radian.
    … …  
    108114void ECEF_2_ENU(double x, double y, double z, double &e, double &n, double &u,
    109115                double lon0, double lat0, double he0);
     116
     117/** Convert from ECEF to ENU.
     118 * @param[in] lon0 Longitude of the origin in radian.
     119 * @param[in] lat0 Latitude of the origin in radian.
     120 * @param[in] he0 Height of the origin in radian.
     121 */
     122void ENU_2_ECEF(double e, double n, double u,double &x, double &y, double &z,
     123                double lon0, double lat0, double he0);
     124
    110125////////////////////////////////////////////////////////////////////////
    111126
Note: See TracChangeset for help on using the changeset viewer.