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