source: flair-src/trunk/lib/FlairSensorActuator/src/NeoM8N.cpp @ 330

Last change on this file since 330 was 268, checked in by Sanahuja Guillaume, 3 years ago

add defines for architectures to speed up compile time

File size: 3.3 KB
Line 
1// %flair:license{
2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
4// %flair:license}
5//  created:    2017/08/30
6//  filename:   NeoM8N.cpp
7//
8//  author:     Guillaume Sanahuja
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
13//  purpose:    objet integrant le recepteur gps ublox NeoM8N
14//
15//
16/*********************************************************************/
17#ifdef ARMV7A
18
19#include "NeoM8N.h"
20#include <SerialPort.h>
21#include <FrameworkManager.h>
22#include <string.h>
23
24using std::string;
25using namespace flair::core;
26
27namespace flair {
28namespace sensor {
29
30NeoM8N::NeoM8N(string name,
31             SerialPort *serialport,NmeaGps::NMEAFlags_t NMEAFlags,uint8_t priority)
32    : NmeaGps(name, NMEAFlags), Thread(getFrameworkManager(), name, priority) {
33  this->serialport = serialport;
34  serialport->SetRxTimeout(100000000);
35 
36  SetIsReady(true);
37}
38
39NeoM8N::~NeoM8N() {
40  SafeStop();
41  Join();
42}
43
44void NeoM8N::Run(void) {
45 
46  //turn off all messages
47  {
48    char tx[]={0x06, 0x01, 0x03, 0, 0xf0, 0,0};
49    for(int i=0;i<0x100;i++) {
50      tx[5]=i;//msg id
51      SendUbx(tx,sizeof(tx));
52    }
53  }
54  //10Hz output
55  {
56    char tx[]={0x06, 0x08, 0x06, 0, 0x64, 0, 0x01, 0, 0, 0};
57    SendUbx(tx,sizeof(tx));
58  }
59 
60  if ((NMEAFlags & GGA) != 0) {
61    char tx[]={0x06, 0x01, 0x03, 0, 0xf0, 0,0x01};
62    SendUbx(tx,sizeof(tx));
63  }
64  if ((NMEAFlags & VTG) != 0) {
65    char tx[]={0x06, 0x01, 0x03, 0, 0xf0, 0x05,0x01};
66    SendUbx(tx,sizeof(tx));
67  }
68  if ((NMEAFlags & GST) != 0) {
69    char tx[]={0x06, 0x01, 0x03, 0, 0xf0, 0x07,0x01};
70    SendUbx(tx,sizeof(tx));
71  }
72  if ((NMEAFlags & GSA) != 0) {
73    char tx[]={0x06, 0x01, 0x03, 0, 0xf0, 0x02,0x01};
74    SendUbx(tx,sizeof(tx));
75  }
76 
77  /** Debut running loop **/
78  WarnUponSwitches(true);
79
80  Sync();
81 
82  while (!ToBeStopped()) {
83    char response[1024] = {0};
84    int size = 0;
85    while (!ToBeStopped()) {
86      ssize_t read = serialport->Read(&response[size], 1);
87      if (read < 0) {
88        Thread::Err("erreur Read (%s)\n", strerror(-read));
89      }
90
91      if (response[size] == 0x0a ) break;
92      size++;
93      if (size==sizeof(response)) break;
94    }
95    if (size!=sizeof(response)) {
96      size++;
97      parseFrame(response, size);
98    } else {
99      Thread::Warn("frame too long for buffer\n");
100    }
101  }
102  /** fin running loop **/
103  WarnUponSwitches(false);
104}
105
106void NeoM8N::SendUbx(char* buf,size_t length) {
107  char header[2]={0xb5,0x62};
108  int written=serialport->Write(header, sizeof(header));
109  if (written< 0) {
110      Thread::Err("erreur Write (%s)\n", strerror(-written));
111  }
112 
113  written=serialport->Write(buf, length);
114  if (written< 0) {
115      Thread::Err("erreur Write (%s)\n", strerror(-written));
116  }
117 
118  unsigned char checksum[2]={0,0};
119  for(int i=0;i<length;i++) {
120     checksum[0] = checksum[0] + buf[i];
121     checksum[1] = checksum[1] + checksum[0];
122  }
123  written=serialport->Write(checksum, sizeof(checksum));
124  if (written< 0) {
125      Thread::Err("erreur Write (%s)\n", strerror(-written));
126  }
127}
128
129void NeoM8N::Sync(void) {
130  char data = 0;
131  ssize_t read = 0;
132
133  // attente fin trame
134  while (data != 0x0a && !ToBeStopped()) {
135    read = serialport->Read(&data, 1);
136    SleepMS(10);
137
138    if (read < 0) {
139      Thread::Err("erreur Read (%s)\n", strerror(-read));
140    }
141  }
142}
143
144} // end namespace sensor
145} // end namespace flair
146
147#endif
Note: See TracBrowser for help on using the repository browser.