Changeset 55 in flair-src

Timestamp:

Jul 28, 2016, 5:55:31 PM (8 years ago)

Author:

Sanahuja Guillaume

Message:

simu gps

Location:

trunk/lib

Files:

• FlairCore/src/FrameworkManager.cpp (modified) (2 diffs)
• FlairCore/src/FrameworkManager.h (modified) (3 diffs)
• FlairCore/src/FrameworkManager_impl.cpp (modified) (3 diffs)
• FlairCore/src/unexported/FrameworkManager_impl.h (modified) (1 diff)
• FlairSensorActuator/src/Imu.cpp (modified) (1 diff)
• FlairSensorActuator/src/NmeaGps.cpp (modified) (11 diffs)
• FlairSensorActuator/src/NmeaGps.h (modified) (3 diffs)
• FlairSensorActuator/src/SimuGps.cpp (modified) (4 diffs)
• FlairSensorActuator/src/SimuGps.h (modified) (4 diffs)
• FlairSensorActuator/src/SimuImu.cpp (modified) (3 diffs)
• FlairSensorActuator/src/SimuImu.h (modified) (2 diffs)
• FlairSensorActuator/src/geodesie.cpp (modified) (2 diffs)
• FlairSensorActuator/src/unexported/geodesie.h (modified) (2 diffs)

trunk/lib/FlairCore/src/FrameworkManager.cpp

@@ -133 +133 @@
 }
 
-void FrameworkManager::SetupConnection(string address, uint16_t port,
+void FrameworkManager::SetupConnection(string address, uint16_t port,Time watchdogTimeout,
                                        size_t rcv_buf_size) {
   pimpl_->SetupConnection(address, port, rcv_buf_size);
@@ -143 +143 @@
 
 gui::TabWidget *FrameworkManager::GetTabWidget(void) const {
+  if(pimpl_->tabwidget==NULL) Err("SetupUserInterface must be called before\n");
   return pimpl_->tabwidget;
 }

trunk/lib/FlairCore/src/FrameworkManager.h

@@ -19 +19 @@
 
 namespace flair {
-namespace gui {
-class TabWidget;
-class SendData;
-}
+  namespace gui {
+    class TabWidget;
+    class SendData;
+  }
 }
 namespace flair {
@@ -69 +69 @@
   *
   * Call this method just after the constructor of this class. If this method is
-  *not called, the program will run headless.
+  * not called, the program will run headless.
+  * If this method is called, SetupUserInterface must also be called after this.
   *
   * \param address address of ground station
   * \param port port of ground station
+  * \param watchdogTimeout watchdog timeout for the connection, passing TIME_INFINITE will disable the watchdog
   * \param rcv_buf_size receive buffer size
   */
-  void SetupConnection(std::string address, uint16_t port,
+  void SetupConnection(std::string address, uint16_t port,Time watchdogTimeout=(Time)1000000000,
                        size_t rcv_buf_size = 10000);
 
@@ -82 +84 @@
   *
   * If this method is called after SetupConnection, Widgets will be displayed in
-  *the ground station.
-  * Otherwise, it will run headless, but default values of Widgets will be taken
-  *from the xml file.
+  * the ground station.
+  * If this method is called and SetupConnection was not called, it will run headless but default values of Widgets will be taken
+  * from the xml file.
+  * If this method is not called, Widgets will not be available. Constructing an object based on Widget class will fail.
   *
   * \param xml_file xml file for default values of Widgets

trunk/lib/FlairCore/src/FrameworkManager_impl.cpp

@@ -103 +103 @@
   gcs_watchdog = NULL;
   _this = this;
+  tabwidget=NULL;
 
   // Avoids memory swapping for this program
@@ -193 +194 @@
 }
 
-void FrameworkManager_impl::SetupConnection(string address, uint16_t port,
+void FrameworkManager_impl::SetupConnection(string address, uint16_t port,Time watchdogTimeout,
                                             size_t rcv_buf_size) {
   UDT::startup();
@@ -804 +805 @@
         if (tmp.running == true) // start logging
         {
-          filename =
-              caller->log_path + "/" + caller->FileName(tmp.device) + ".dbt";
-          printf("Creating log file %s (log size %i)\n", filename.c_str(),
-                 (int)tmp.size);
+          filename = caller->log_path + "/" + caller->FileName(tmp.device) + ".dbt";
+          printf("Creating log file %s (log size %i)\n", filename.c_str(), (int)tmp.size);
           tmp.dbtFile = inithdFile((char *)filename.c_str(), UAV, tmp.size);
           logs.push_back(tmp);

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

@@ -43 +43 @@
   FrameworkManager_impl(flair::core::FrameworkManager *self, std::string name);
   ~FrameworkManager_impl();
-  void SetupConnection(std::string address, uint16_t port,
+  void SetupConnection(std::string address, uint16_t port,flair::core::Time watchdogTimeout,
                        size_t rcv_buf_size = 10000);
   void SetupUserInterface(std::string xml_file);

trunk/lib/FlairSensorActuator/src/Imu.cpp

@@ -59 +59 @@
 
 void Imu::GetDatas(ImuData **outImuData) const { *outImuData = imuData; }
+
 void Imu::UpdateImu() {
   if (rotation == NULL) {

trunk/lib/FlairSensorActuator/src/NmeaGps.cpp

@@ -47 +47 @@
   nmea_zero_INFO(&info);
   nmea_parser_init(&parser);
-  alt_ref = 0;
+  altRef = 0;
 
   if ((NMEAFlags & GGA) == 0) {
@@ -63 +63 @@
 
   // station sol
-  main_tab = new Tab(parent->GetTabWidget(), name);
-  tab = new TabWidget(main_tab->NewRow(), name);
-  sensor_tab = new Tab(tab, "Reglages");
-  GroupBox *reglages_groupbox = new GroupBox(sensor_tab->NewRow(), name);
-  button_ref = new PushButton(reglages_groupbox->NewRow(), "set ref");
-  nb_sat_label = new Label(reglages_groupbox->NewRow(), "nb_sat");
-  fix_label = new Label(reglages_groupbox->LastRowLastCol(), "fix");
+  mainTab = new Tab(parent->GetTabWidget(), name);
+  tab = new TabWidget(mainTab->NewRow(), name);
+  sensorTab = new Tab(tab, "Setup");
+  setupGroupbox = new GroupBox(sensorTab->NewRow(), name);
+  buttonRef = new PushButton(setupGroupbox->NewRow(), "set ref");
+  nbSatLabel = new Label(setupGroupbox->NewRow(), "nb_sat");
+  fixLabel = new Label(setupGroupbox->LastRowLastCol(), "fix");
 
   position = new GeoCoordinate((IODevice *)this, "position", 0, 0, 0);
 
-  take_ref = false;
+  takeRef = false;
 
   gpsData = new GpsData(this);
   AddDataToLog(gpsData);
 
-  nb_sat_label->SetText("nb_sat: %i", gpsData->GetNumberOfSatellites());
-  fix_label->SetText("fix: %i", gpsData->GetFixQuality());
+  nbSatLabel->SetText("number of satellies: %i", gpsData->GetNumberOfSatellites());
+  fixLabel->SetText("fix quality: %i", gpsData->GetFixQuality());
+}
+
+NmeaGps::NmeaGps(const IODevice *parent, std::string name) : IODevice(parent, name) {
+  gpsData = new GpsData(this);
+  mainTab = NULL;
+  tab = NULL;
+  sensorTab = NULL;
 }
 
 NmeaGps::~NmeaGps() {
-  nmea_parser_destroy(&parser);
-  delete main_tab;
-}
+  if (mainTab != NULL) {
+    nmea_parser_destroy(&parser);
+    delete mainTab;
+  }
+}
+
+GroupBox *NmeaGps::GetGroupBox(void) const { return setupGroupbox; }
 
 const GpsData *NmeaGps::GetDatas(void) const {
@@ -96 +107 @@
 
 void NmeaGps::UseDefaultPlot(void) {
-  plot_tab = new Tab(tab, "Mesures");
-
-  if ((NMEAFlags & GGA) != 0) {
-    e_plot = new DataPlot1D(plot_tab->NewRow(), "e", -10, 10);
-    e_plot->AddCurve(gpsData->Element(GpsData::East));
-    n_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "n", -10, 10);
-    n_plot->AddCurve(gpsData->Element(GpsData::North));
-    u_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "u", -10, 10);
-    u_plot->AddCurve(gpsData->Element(GpsData::Up));
+  plotTab = new Tab(tab, "Mesures");
+
+  if ((NMEAFlags & GGA) != 0) {
+    ePlot = new DataPlot1D(plotTab->NewRow(), "e", -10, 10);
+    ePlot->AddCurve(gpsData->Element(GpsData::East));
+    nPlot = new DataPlot1D(plotTab->LastRowLastCol(), "n", -10, 10);
+    nPlot->AddCurve(gpsData->Element(GpsData::North));
+    uPlot = new DataPlot1D(plotTab->LastRowLastCol(), "u", -10, 10);
+    uPlot->AddCurve(gpsData->Element(GpsData::Up));
   }
   if ((NMEAFlags & VTG) != 0) {
-    ve_plot = new DataPlot1D(plot_tab->NewRow(), "ve", -10, 10);
-    ve_plot->AddCurve(gpsData->Element(GpsData::EastVelocity));
-    vn_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "vn", -10, 10);
-    vn_plot->AddCurve(gpsData->Element(GpsData::NorthVelocity));
+    vePlot = new DataPlot1D(plotTab->NewRow(), "ve", -10, 10);
+    vePlot->AddCurve(gpsData->Element(GpsData::EastVelocity));
+    vnPlot = new DataPlot1D(plotTab->LastRowLastCol(), "vn", -10, 10);
+    vnPlot->AddCurve(gpsData->Element(GpsData::NorthVelocity));
   }
 
@@ -120 +131 @@
 DataPlot1D *NmeaGps::EPlot(void) const {
   if ((NMEAFlags & GGA) != 0) {
-    return e_plot;
+    return ePlot;
   } else {
     Err("GGA sentence not requested\n");
@@ -129 +140 @@
 DataPlot1D *NmeaGps::NPlot(void) const {
   if ((NMEAFlags & GGA) != 0) {
-    return n_plot;
+    return nPlot;
   } else {
     Err("GGA sentence not requested\n");
@@ -138 +149 @@
 DataPlot1D *NmeaGps::UPlot(void) const {
   if ((NMEAFlags & GGA) != 0) {
-    return u_plot;
+    return uPlot;
   } else {
     Err("GGA sentence not requested\n");
@@ -147 +158 @@
 DataPlot1D *NmeaGps::VEPlot(void) const {
   if ((NMEAFlags & VTG) != 0) {
-    return ve_plot;
+    return vePlot;
   } else {
     Err("GGA sentence not requested\n");
@@ -156 +167 @@
 DataPlot1D *NmeaGps::VNPlot(void) const {
   if ((NMEAFlags & VTG) != 0) {
-    return vn_plot;
-  } else {
-    Err("GGA sentence not requested\n");
-    return NULL;
-  }
-}
-
-Layout *NmeaGps::GetLayout(void) const { return sensor_tab; }
-
-Tab *NmeaGps::GetPlotTab(void) const { return plot_tab; }
+    return vnPlot;
+  } else {
+    Err("GGA sentence not requested\n");
+    return NULL;
+  }
+}
+
+Layout *NmeaGps::GetLayout(void) const { return sensorTab; }
+
+Tab *NmeaGps::GetPlotTab(void) const { return plotTab; }
 
 TabWidget *NmeaGps::GetTab(void) const { return tab; }
 
-void NmeaGps::SetRef(void) { take_ref = true; }
+void NmeaGps::SetRef(void) { takeRef = true; }
 
 void NmeaGps::GetEnu(Vector3D *point) {
@@ -197 +208 @@
     if (gpsData->GetFixQuality() != (GpsData::FixQuality_t)pack.sig) {
       gpsData->SetFixQuality((GpsData::FixQuality_t)pack.sig);
-      fix_label->SetText("fix: %i", pack.sig);
+      fixLabel->SetText("fix: %i", pack.sig);
     }
     if (gpsData->GetNumberOfSatellites() != pack.satinuse) {
       gpsData->SetNumberOfSatellites(pack.satinuse) ;
-      nb_sat_label->SetText("nb_sat: %i", pack.satinuse);
+      nbSatLabel->SetText("nb_sat: %i", pack.satinuse);
     }
     gpsData->ReleaseMutex();
@@ -209 +220 @@
                              info.elv);
 
-    if ((info.sig == 2 && alt_ref == 0) || button_ref->Clicked() == true ||
-        take_ref == true) {
+    if ((info.sig == 2 && altRef == 0) || buttonRef->Clicked() == true ||
+        takeRef == true) {
       Printf("prise pos ref\n");
-      lat_ref = pos.lat;
-      long_ref = pos.lon;
-      alt_ref = info.elv;
-      take_ref = false;
+      latRef = pos.lat;
+      longRef = pos.lon;
+      altRef = info.elv;
+      takeRef = false;
     }
     // if(alt_ref!=0)
@@ -222 +233 @@
       double e, n, u;
       Geographique_2_ECEF(pos.lon, pos.lat, info.elv, x, y, z);
-      ECEF_2_ENU(x, y, z, e, n, u, long_ref, lat_ref, alt_ref);
+      ECEF_2_ENU(x, y, z, e, n, u, longRef, latRef, altRef);
       // Printf("lon:%f lat:%f elv:%f\n",pos.lon,pos.lat,info.elv);
 

trunk/lib/FlairSensorActuator/src/NmeaGps.h

@@ -32 +32 @@
     class Map;
     class Label;
+    class GroupBox;
   }
 }
@@ -67 +68 @@
 
   /*!
+  * \brief Constructor
+  *
+  * Construct a NmeaGps. \n
+  * This contructor must only be called for a simulated device.
+  *
+  * \param parent parent
+  * \param name name
+  */
+  NmeaGps(const core::IODevice *parent, std::string name);
+
+  /*!
   * \brief Destructor
   *
@@ -173 +185 @@
   NMEAFlags_t NMEAFlags;
 
-protected:
+  /*!
+  * \brief Get GPS datas
+  *
+  * \param gpsData GPS datas
+  */
+  void GetDatas(core::GpsData **gpsData) const;
+
+  /*!
+  * \brief Setup GroupBox
+  *
+  * \return setup GroupBox
+  */
+  gui::GroupBox *GetGroupBox(void) const;
+
+private:
+  gui::Tab *mainTab, *sensorTab;
+  gui::TabWidget *tab;
+  gui::GroupBox *setupGroupbox;
+  gui::PushButton *buttonRef;
+  gui::DataPlot1D *ePlot;
+  gui::DataPlot1D *nPlot;
+  gui::DataPlot1D *uPlot;
+  gui::DataPlot1D *vePlot;
+  gui::DataPlot1D *vnPlot;
+  gui::Tab *plotTab;
+  gui::Map *map;
+  gui::Label *nbSatLabel, *fixLabel;
   core::GeoCoordinate *position;
-
-  /*!
-  * \brief Get GPS datas
-  *
-  * \param gpsData GPS datas
-  */
-  void GetDatas(core::GpsData **gpsData) const;
-
-private:
-  /*!
-  * \brief Update using provided datas
-  *
-  * Reimplemented from IODevice.
-  *
-  * \param data data from the parent to process
-  */
-  void UpdateFrom(const core::io_data *data){};
-
-  gui::Tab *main_tab, *sensor_tab;
-  gui::TabWidget *tab;
-  gui::PushButton *button_ref;
-  gui::DataPlot1D *e_plot;
-  gui::DataPlot1D *n_plot;
-  gui::DataPlot1D *u_plot;
-  gui::DataPlot1D *ve_plot;
-  gui::DataPlot1D *vn_plot;
-  gui::Tab *plot_tab;
-  gui::Map *map;
-  gui::Label *nb_sat_label, *fix_label;
-  bool take_ref;
+  bool takeRef;
   nmeaINFO info;
   nmeaPARSER parser;
   nmeaGPGGA pack;
   nmeaPOS pos;
-  double lat_ref, long_ref, alt_ref;
+  double latRef, longRef, altRef;
   core::GpsData* gpsData;
 };

trunk/lib/FlairSensorActuator/src/SimuGps.cpp

@@ -18 +18 @@
 #include "SimuGps.h"
 #include <FrameworkManager.h>
-#include <string.h>
 #include <GeoCoordinate.h>
 #include <GpsData.h>
+#include <SharedMem.h>
+#include <SpinBox.h>
+#include <DoubleSpinBox.h>
+#include <GroupBox.h>
+#include <Euler.h>
+#include <cvmatrix.h>
+#include <sstream>
+#include "geodesie.h"
 
 using std::string;
+using std::ostringstream;
 using namespace flair::core;
+using namespace flair::gui;
+using namespace Geodesie;
 
 namespace flair {
@@ -29 +39 @@
 
 SimuGps::SimuGps(const FrameworkManager *parent, string name,
-                 NmeaGps::NMEAFlags_t NMEAFlags, uint8_t priority)
-    : Thread(parent, name, priority), NmeaGps(parent, name, NMEAFlags) {}
+                 NmeaGps::NMEAFlags_t NMEAFlags, uint32_t deviceId,uint8_t priority)
+    : NmeaGps(parent, name, NMEAFlags),Thread(parent, name, priority) {
+
+  dataRate = new SpinBox(GetGroupBox()->NewRow(), "data rate", " Hz", 1, 500, 1, 200);
+  latitudeRef = new DoubleSpinBox(GetGroupBox()->NewRow(), "latitude ref", " deg", -90, 90, 1, 6,49.402313);
+  longitudeRef = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "longitude ref", " deg", -180, 180, 1, 6,2.795463);
+  altitudeRef= new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "altitude ref", " m", 0, 6000, 100, 1,0);
+  fixQuality = new SpinBox(GetGroupBox()->NewRow(), "fix quality", 1, 8, 1, 1);
+  numberOfSatellites = new SpinBox(GetGroupBox()->NewRow(), "number of satellites", 1, 15, 1, 5);
+
+  ostringstream dev_name;
+  dev_name << "simu_gps_" << deviceId;
+  shmem = new SharedMem((Thread *)this, dev_name.str().c_str(),
+                        sizeof(gps_states_t));
+}
+
+
+SimuGps::SimuGps(const IODevice *parent, string name, uint32_t deviceId)
+    : NmeaGps(parent, name), Thread(parent, name, 0) {
+  dataRate = NULL;
+
+  ostringstream dev_name;
+  dev_name << "simu_gps_" << deviceId;
+  shmem = new SharedMem((Thread *)this, dev_name.str().c_str(),
+                        sizeof(gps_states_t));
+}
 
 SimuGps::~SimuGps() {
@@ -37 +71 @@
 }
 
+void SimuGps::UpdateFrom(const io_data *data) {
+  if (data != NULL) {
+    cvmatrix *input = (cvmatrix *)data;
+    gps_states_t state;
+
+    input->GetMutex();
+    //simulator is ned, convert it to enu
+    //TODO: put simulator in enu?
+    state.x = input->ValueNoMutex(5, 0);
+    state.y = input->ValueNoMutex(4, 0);
+    state.z = -input->ValueNoMutex(6, 0);
+    input->ReleaseMutex();
+
+    shmem->Write((char *)&state, sizeof(gps_states_t));
+  }
+}
+
 void SimuGps::Run(void) {
-  // double lat=0;
+  gps_states_t state;
   char buf[500];
   nmeaGPGGA gga;
   nmeaGPVTG vtg;
-  SetPeriodMS(500);
+  nmeaPOS pos;
+  nmeaINFO info;
+
+  if (dataRate == NULL) {
+    Thread::Err("not applicable for simulation part.\n");
+    return;
+  }
+
+  SetPeriodUS((uint32_t)(1000000. / dataRate->Value()));
+
   WarnUponSwitches(true);
-
-  gga.sig=1;
-  gga.satinuse=2;
-  gga.lat=49.402313;
-  gga.lon=2.795463;
 
   vtg.spn=1;
@@ -56 +111 @@
     WaitPeriod();
 
+    if (dataRate->ValueChanged() == true) {
+      SetPeriodUS((uint32_t)(1000000. / dataRate->Value()));
+    }
+
+    shmem->Read((char *)&state, sizeof(gps_states_t));
+
+    double x, y, z;
+    ENU_2_ECEF(state.x, state.y, state.z, x,y,z, Euler::ToRadian(longitudeRef->Value()), Euler::ToRadian(latitudeRef->Value()), altitudeRef->Value());
+    ECEF_2_Geographique( x, y, z,pos.lon, pos.lat, gga.elv);
+    nmea_pos2info(&pos,&info);
+
+    if(pos.lat>0) {
+      gga.ns='N';
+      gga.lat=info.lat;
+    } else {
+      gga.ns='S';
+      gga.lat=-info.lat;
+    }
+    if(pos.lon>0) {
+      gga.ew='E';
+      gga.lon=info.lon;
+    } else {
+      gga.ew='W';
+      gga.lon=-info.lon;
+    }
+
+    gga.sig=fixQuality->Value();
+    gga.satinuse=numberOfSatellites->Value();
+
     nmea_gen_GPGGA(buf,sizeof(buf),&gga);
     parseFrame(buf,sizeof(buf));
-    position->SetCoordinates(49.402313, 2.795463, 0);
-    //      lat+=.5;
   }
 

trunk/lib/FlairSensorActuator/src/SimuGps.h

@@ -18 +18 @@
 
 namespace flair {
-namespace core {
-class FrameworkManager;
-}
+  namespace core {
+    class FrameworkManager;
+    class SharedMem;
+  }
+  namespace gui {
+    class SpinBox;
+    class DoubleSpinBox;
+  }
 }
 
@@ -34 +39 @@
   * \brief Constructor
   *
-  * Construct a Novatel.
+  * Construct a simulation GPS. Control part.
   *
   * \param parent parent
   * \param name name
   * \param NMEAFlags NMEA sentances to enable
+  * \param deviceId device id
   * \param priority priority of the Thread
   */
   SimuGps(const core::FrameworkManager *parent, std::string name,
-          NmeaGps::NMEAFlags_t NMEAFlags, uint8_t priority);
+          NmeaGps::NMEAFlags_t NMEAFlags, uint32_t deviceId,uint8_t priority);
+
+  /*!
+  * \brief Constructor
+  *
+  * Construct a simulation GPS. Simulation part.\n
+  * The Thread of this class should not be run.
+  *
+  * \param parent parent
+  * \param name name
+  * \param deviceId device id
+  */
+  SimuGps(const core::IODevice *parent, std::string name, uint32_t deviceId);
 
   /*!
@@ -58 +76 @@
   * \param data data from the parent to process
   */
-  void UpdateFrom(const core::io_data *data){};
+  void UpdateFrom(const core::io_data *data);
 
   /*!
@@ -67 +85 @@
   */
   void Run(void);
+
+  typedef struct {
+    float x;
+    float y;
+    float z;
+  } gps_states_t;
+
+  core::SharedMem *shmem;
+  gui::SpinBox *dataRate,*fixQuality,*numberOfSatellites;
+  gui::DoubleSpinBox *latitudeRef,*longitudeRef,*altitudeRef;
 };
 } // end namespace sensor

trunk/lib/FlairSensorActuator/src/SimuImu.cpp

@@ -37 +37 @@
                  uint8_t priority)
     : Imu(parent, name), Thread(parent, name, priority) {
-  data_rate =
+  dataRate =
       new SpinBox(GetGroupBox()->NewRow(), "data rate", " Hz", 1, 500, 1, 200);
   ahrsData = new AhrsData((Imu *)this);
@@ -49 +49 @@
 SimuImu::SimuImu(const IODevice *parent, string name, uint32_t dev_id)
     : Imu(parent, name), Thread(parent, name, 0) {
-  data_rate = NULL;
+  dataRate = NULL;
 
   ostringstream dev_name;
@@ -86 +86 @@
   GetDatas(&imuData);
 
-  if (data_rate == NULL) {
+  if (dataRate == NULL) {
     Thread::Err("not applicable for simulation part.\n");
     return;
   }
 
-  SetPeriodUS((uint32_t)(1000000. / data_rate->Value()));
+  SetPeriodUS((uint32_t)(1000000. / dataRate->Value()));
 
   while (!ToBeStopped()) {
     WaitPeriod();
 
-    if (data_rate->ValueChanged() == true) {
-      SetPeriodUS((uint32_t)(1000000. / data_rate->Value()));
+    if (dataRate->ValueChanged() == true) {
+      SetPeriodUS((uint32_t)(1000000. / dataRate->Value()));
     }
 

trunk/lib/FlairSensorActuator/src/SimuImu.h

@@ -18 +18 @@
 
 namespace flair {
-namespace core {
-class SharedMem;
-class AhrsData;
-}
-namespace gui {
-class SpinBox;
-}
+  namespace core {
+    class SharedMem;
+    class AhrsData;
+  }
+  namespace gui {
+    class SpinBox;
+  }
 }
 
@@ -93 +93 @@
     float wz;
   } imu_states_t;
-  gui::SpinBox *data_rate;
+
+  gui::SpinBox *dataRate;
   core::SharedMem *shmem;
   core::AhrsData *ahrsData;

trunk/lib/FlairSensorActuator/src/geodesie.cpp

@@ -322 +322 @@
 }
 
+//from https://gist.github.com/klucar/1536194
+void Geodesie::ECEF_2_Geographique(double x, double y, double z,
+                                   double &longitude, double &latitude, double &he) {
+
+  double asq=pow(GRS_a, 2);
+  double esq=pow(GRS_e, 2);
+  double b = sqrt( asq * (1-esq) );
+  double bsq = pow(b,2);
+  double ep = sqrt( (asq - bsq)/bsq);
+  double p = sqrt( pow(x,2) + pow(y,2) );
+  double th = atan2(GRS_a*z, b*p);
+
+  longitude = atan2(y,x);
+  latitude = atan2( (z + pow(ep,2)*b*pow(sin(th),3) ), (p - esq*GRS_a*pow(cos(th),3)) );
+  double N = GRS_a/( sqrt(1-esq*pow(sin(latitude),2)) );
+  double alt = p / cos(latitude) - N;
+
+  // mod longitude to 0-2pi
+  if(longitude<0) longitude +=2*M_PI;
+
+  // correction for altitude near poles left out.
+}
+
 ////////////////////////////////////////////////////////////////////////
 void Geodesie::ECEF_2_ENU(double x, double y, double z, double &e, double &n,
@@ -351 +374 @@
   C.Apply(x, y, z, e, n, u);
 }
+
+void Geodesie::ENU_2_ECEF(double e, double n,double u,
+                          double &x, double &y, double &z,
+                          double lon0, double lat0, double he0) {
+  double slat = std::sin(lat0);//phi
+  double clat = std::cos(lat0);
+  double slon = std::sin(lon0);//lambda
+  double clon = std::cos(lon0);
+
+  Geodesie::Matrice C;
+  C.c0_l0 = -slon;
+  C.c1_l0 = -clon*slat;
+  C.c2_l0 = clon*clat;
+
+  C.c0_l1 = clon;
+  C.c1_l1 = -slon * slat;
+  C.c2_l1 = slon*clat;
+
+  C.c0_l2 = 0;
+  C.c1_l2 = clat;
+  C.c2_l2 = slat;
+
+  C.Apply(e, n, u,x,y,z);
+
+  double x0, y0, z0;
+  Geographique_2_ECEF(lon0, lat0, he0, x0, y0, z0);
+
+  x += x0;
+  y += y0;
+  z += z0;
+}

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

@@ -101 +101 @@
 void Geographique_2_ECEF(double longitude, double latitude, double he,
                          double &x, double &y, double &z);
-/** Convert from ECEF two ENU.
+
+/** Convert from ECEF to geographique.
+ */
+void ECEF_2_Geographique(double x, double y, double z,
+                          double &longitude, double &latitude, double &he);
+
+/** Convert from ECEF to ENU.
  * @param[in] lon0 Longitude of the origin in radian.
  * @param[in] lat0 Latitude of the origin in radian.
@@ -108 +114 @@
 void ECEF_2_ENU(double x, double y, double z, double &e, double &n, double &u,
                 double lon0, double lat0, double he0);
+
+/** Convert from ECEF to ENU.
+ * @param[in] lon0 Longitude of the origin in radian.
+ * @param[in] lat0 Latitude of the origin in radian.
+ * @param[in] he0 Height of the origin in radian.
+ */
+void ENU_2_ECEF(double e, double n, double u,double &x, double &y, double &z,
+                double lon0, double lat0, double he0);
+
 ////////////////////////////////////////////////////////////////////////