Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014 #include "AP_GPS_MTK16.h"
00015 #include <stdint.h>
00016
00017
00018 AP_GPS_MTK16::AP_GPS_MTK16(Stream *s) : GPS(s)
00019 {
00020 }
00021
00022
00023 void
00024 AP_GPS_MTK16::init(void)
00025 {
00026 _port->flush();
00027
00028
00029
00030 _port->print(MTK_SET_BINARY);
00031
00032
00033 _port->print(MTK_OUTPUT_4HZ);
00034 }
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047 bool
00048 AP_GPS_MTK16::read(void)
00049 {
00050 uint8_t data;
00051 int numc;
00052 bool parsed = false;
00053
00054 numc = _port->available();
00055 for (int i = 0; i < numc; i++) {
00056
00057
00058 data = _port->read();
00059
00060 restart:
00061 switch(_step){
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072 case 0:
00073 if(PREAMBLE1 == data)
00074 _step++;
00075 break;
00076 case 1:
00077 if (PREAMBLE2 == data) {
00078 _step++;
00079 break;
00080 }
00081 _step = 0;
00082 goto restart;
00083 case 2:
00084 if (sizeof(_buffer) == data) {
00085 _step++;
00086 _ck_b = _ck_a = data;
00087 _payload_counter = 0;
00088 } else {
00089 _step = 0;
00090 goto restart;
00091 }
00092 break;
00093
00094
00095
00096 case 3:
00097 _buffer.bytes[_payload_counter++] = data;
00098 _ck_b += (_ck_a += data);
00099 if (_payload_counter == sizeof(_buffer))
00100 _step++;
00101 break;
00102
00103
00104
00105 case 4:
00106 _step++;
00107 if (_ck_a != data) {
00108 _step = 0;
00109 }
00110 break;
00111 case 5:
00112 _step = 0;
00113 if (_ck_b != data) {
00114 break;
00115 }
00116
00117 fix = _buffer.msg.fix_type == FIX_3D;
00118 latitude = _buffer.msg.latitude * 10;
00119 longitude = _buffer.msg.longitude * 10;
00120 altitude = _buffer.msg.altitude;
00121 ground_speed = _buffer.msg.ground_speed;
00122 ground_course = _buffer.msg.ground_course;
00123 num_sats = _buffer.msg.satellites;
00124 hdop = _buffer.msg.hdop;
00125
00126
00127 time = _swapl(&_buffer.msg.utc_time);
00128
00129 parsed = true;
00130 }
00131 }
00132 return parsed;
00133 }