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

Last change on this file since 196 was 196, checked in by Sanahuja Guillaume, 4 years ago

avoid buffer overrun

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