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

simu gps

File:
1 edited

Legend:

Unmodified
Added
Removed
  • 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
Note: See TracChangeset for help on using the changeset viewer.