Changeset 15 in flair-src for trunk/lib/FlairSensorActuator/src/Mb800.cpp
- Timestamp:
- 04/08/16 15:40:57 (8 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/lib/FlairSensorActuator/src/Mb800.cpp
r3 r15 24 24 using namespace flair::core; 25 25 26 namespace flair 27 { 28 namespace sensor 29 { 26 namespace flair { 27 namespace sensor { 30 28 31 Mb800::Mb800(const FrameworkManager* parent,string name,SerialPort *serialport,Gps::NMEAFlags_t NMEAFlags,uint8_t priority) : Gps(parent,name,NMEAFlags), Thread(parent,name,priority) 32 { 33 this->serialport=serialport; 34 serialport->SetRxTimeout(100000000); 29 Mb800::Mb800(const FrameworkManager *parent, string name, 30 SerialPort *serialport, Gps::NMEAFlags_t NMEAFlags, 31 uint8_t priority) 32 : Gps(parent, name, NMEAFlags), Thread(parent, name, priority) { 33 this->serialport = serialport; 34 serialport->SetRxTimeout(100000000); 35 35 } 36 36 37 Mb800::~Mb800() 38 { 39 SafeStop(); 40 Join(); 37 Mb800::~Mb800() { 38 SafeStop(); 39 Join(); 41 40 } 42 41 43 void Mb800::Run(void) 44 { 45 /** Debut init **/ 46 char response[200] = {0}; 47 int size; 42 void Mb800::Run(void) { 43 /** Debut init **/ 44 char response[200] = {0}; 45 int size; 48 46 49 47 /** Fin init **/ 50 48 51 /** Debut config **/ 52 char to_send[]="$PASHS,NME,ALL,A,OFF\r\n"; 49 /** Debut config **/ 50 char to_send[] = "$PASHS,NME,ALL,A,OFF\r\n"; 51 serialport->Write(to_send, sizeof(to_send)); 52 53 { 54 char to_send[] = "$PASHS,CPD,AFP,95.0\r\n"; 53 55 serialport->Write(to_send, sizeof(to_send)); 56 } 57 { 58 char to_send[] = "$PASHS,DIF,PRT,C,RT3\r\n"; 59 serialport->Write(to_send, sizeof(to_send)); 60 } 54 61 55 { 56 char to_send[]="$PASHS,CPD,AFP,95.0\r\n"; 57 serialport->Write(to_send, sizeof(to_send)); 62 if ((NMEAFlags & GGA) != 0) { 63 char to_send[] = "$PASHS,NME,GGA,A,ON,0.05\r\n"; 64 size = serialport->Write(to_send, sizeof(to_send)); 65 // Printf("ecrit %i\n",size); 66 } 67 if ((NMEAFlags & VTG) != 0) { 68 char to_send[] = "$PASHS,NME,VTG,A,ON,0.05\r\n"; 69 size = serialport->Write(to_send, sizeof(to_send)); 70 // Printf("%i\n",size); 71 } 72 if ((NMEAFlags & GST) != 0) { 73 char to_send[] = "$PASHS,NME,GST,A,ON,0.05\r\n"; 74 size = serialport->Write(to_send, sizeof(to_send)); 75 // Printf("%i\n",size); 76 } 77 78 Sync(); 79 80 /** Fin config **/ 81 82 /** Debut running loop **/ 83 WarnUponSwitches(true); 84 85 while (!ToBeStopped()) { 86 SleepMS(10); 87 size = 0; 88 while (!ToBeStopped()) { 89 ssize_t read = serialport->Read(&response[size], 1); 90 if (read < 0) { 91 Thread::Err("erreur Read (%s)\n", strerror(-read)); 92 } 93 94 if (response[size] == 0x0a) 95 break; 96 size++; 58 97 } 59 { 60 char to_send[]="$PASHS,DIF,PRT,C,RT3\r\n"; 61 serialport->Write(to_send, sizeof(to_send)); 62 } 63 64 if((NMEAFlags&GGA)!=0) 65 { 66 char to_send[]="$PASHS,NME,GGA,A,ON,0.05\r\n"; 67 size=serialport->Write(to_send, sizeof(to_send)); 68 //Printf("ecrit %i\n",size); 69 } 70 if((NMEAFlags&VTG)!=0) 71 { 72 char to_send[]="$PASHS,NME,VTG,A,ON,0.05\r\n"; 73 size=serialport->Write(to_send, sizeof(to_send)); 74 //Printf("%i\n",size); 75 } 76 if((NMEAFlags&GST)!=0) 77 { 78 char to_send[]="$PASHS,NME,GST,A,ON,0.05\r\n"; 79 size=serialport->Write(to_send, sizeof(to_send)); 80 //Printf("%i\n",size); 81 } 82 83 Sync(); 84 85 /** Fin config **/ 86 87 /** Debut running loop **/ 88 WarnUponSwitches(true); 89 90 while(!ToBeStopped()) 91 { 92 SleepMS(10); 93 size=0; 94 while(!ToBeStopped()) 95 { 96 ssize_t read = serialport->Read(&response[size],1); 97 if(read<0) 98 { 99 Thread::Err("erreur Read (%s)\n",strerror(-read)); 100 } 101 102 if(response[size]==0x0a) break; 103 size++; 104 } 105 size++; 106 parseFrame(response, size); 107 } 108 /** fin running loop **/ 109 WarnUponSwitches(false); 98 size++; 99 parseFrame(response, size); 100 } 101 /** fin running loop **/ 102 WarnUponSwitches(false); 110 103 } 111 104 112 void Mb800::Sync(void) 113 { 114 char data=0; 115 ssize_t read = 0; 105 void Mb800::Sync(void) { 106 char data = 0; 107 ssize_t read = 0; 116 108 117 //attente fin trame 118 while(data!=0x0a) 119 { 120 read = serialport->Read(&data,1); 121 if(read<0) 122 { 123 Thread::Err("erreur Read (%s)\n",strerror(-read)); 124 } 109 // attente fin trame 110 while (data != 0x0a) { 111 read = serialport->Read(&data, 1); 112 if (read < 0) { 113 Thread::Err("erreur Read (%s)\n", strerror(-read)); 125 114 } 115 } 126 116 } 127 117
Note:
See TracChangeset
for help on using the changeset viewer.