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

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

neom8n

File size: 3.2 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 char response[200] = {0};
45 int size,written;
46
47 //turn off all mesages
48 {
49 char tx[]={0x06, 0x01, 0x03, 0, 0xf0, 0,0};
50 for(int i=0;i<0x100;i++) {
51 tx[5]=i;//msg id
52 SendUbx(tx,sizeof(tx));
53 }
54 }
55 //10Hz output
56 {
57 char tx[]={0x06, 0x08, 0x06, 0, 0x64, 0, 0x01, 0, 0, 0};
58 SendUbx(tx,sizeof(tx));
59 }
60
61 if ((NMEAFlags & GGA) != 0) {
62 char tx[]={0x06, 0x01, 0x03, 0, 0xf0, 0,0x01};
63 SendUbx(tx,sizeof(tx));
64 }
65 if ((NMEAFlags & VTG) != 0) {
66 char tx[]={0x06, 0x01, 0x03, 0, 0xf0, 0x05,0x01};
67 SendUbx(tx,sizeof(tx));
68 }
69 if ((NMEAFlags & GST) != 0) {
70 char tx[]={0x06, 0x01, 0x03, 0, 0xf0, 0x07,0x01};
71 SendUbx(tx,sizeof(tx));
72 }
73 if ((NMEAFlags & GSA) != 0) {
74 char tx[]={0x06, 0x01, 0x03, 0, 0xf0, 0x02,0x01};
75 SendUbx(tx,sizeof(tx));
76 }
77
78 /** Debut running loop **/
79 WarnUponSwitches(true);
80
81 Sync();
82
83 while (!ToBeStopped()) {
84 //SleepMS(10);??
85 size = 0;
86 while (!ToBeStopped()) {
87 ssize_t read = serialport->Read(&response[size], 1);
88 if (read < 0) {
89 Thread::Err("erreur Read (%s)\n", strerror(-read));
90 }
91
92 if (response[size] == 0x0a)
93 break;
94 size++;
95 }
96 size++;
97 parseFrame(response, size);
98 }
99 /** fin running loop **/
100 WarnUponSwitches(false);
101}
102
103void NeoM8N::SendUbx(char* buf,size_t length) {
104 char header[2]={0xb5,0x62};
105 int written=serialport->Write(header, sizeof(header));
106 if (written< 0) {
107 Thread::Err("erreur Write (%s)\n", strerror(-written));
108 }
109
110 written=serialport->Write(buf, length);
111 if (written< 0) {
112 Thread::Err("erreur Write (%s)\n", strerror(-written));
113 }
114
115 unsigned char checksum[2]={0,0};
116 for(int i=0;i<length;i++) {
117 checksum[0] = checksum[0] + buf[i];
118 checksum[1] = checksum[1] + checksum[0];
119 }
120 written=serialport->Write(checksum, sizeof(checksum));
121 if (written< 0) {
122 Thread::Err("erreur Write (%s)\n", strerror(-written));
123 }
124}
125
126void NeoM8N::Sync(void) {
127 char data = 0;
128 ssize_t read = 0;
129
130 // attente fin trame
131 while (data != 0x0a && !ToBeStopped()) {
132 read = serialport->Read(&data, 1);
133 SleepMS(10);
134
135 if (read < 0) {
136 Thread::Err("erreur Read (%s)\n", strerror(-read));
137 }
138 }
139}
140
141} // end namespace sensor
142} // end namespace flair
Note: See TracBrowser for help on using the repository browser.