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

Last change on this file 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.