Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012 #include "AP_GPS_UBLOX.h"
00013 #include <stdint.h>
00014
00015
00016
00017 AP_GPS_UBLOX::AP_GPS_UBLOX(Stream *s) : GPS(s)
00018 {
00019 }
00020
00021
00022
00023 void
00024 AP_GPS_UBLOX::init(void)
00025 {
00026
00027
00028
00029 _port->flush();
00030 }
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041 bool
00042 AP_GPS_UBLOX::read(void)
00043 {
00044 uint8_t data;
00045 int numc;
00046 bool parsed = false;
00047
00048 numc = _port->available();
00049 for (int i = 0; i < numc; i++){
00050
00051
00052 data = _port->read();
00053
00054 switch(_step){
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065 case 1:
00066 if (PREAMBLE2 == data) {
00067 _step++;
00068 break;
00069 }
00070 _step = 0;
00071
00072 case 0:
00073 if(PREAMBLE1 == data)
00074 _step++;
00075 break;
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086 case 2:
00087 _step++;
00088 if (CLASS_NAV == data) {
00089 _gather = true;
00090 _ck_b = _ck_a = data;
00091 } else {
00092 _gather = false;
00093 }
00094 break;
00095 case 3:
00096 _step++;
00097 _ck_b += (_ck_a += data);
00098 _msg_id = data;
00099 if (_gather) {
00100 switch(data) {
00101 case MSG_POSLLH:
00102 _expect = sizeof(ubx_nav_posllh);
00103 break;
00104 case MSG_STATUS:
00105 _expect = sizeof(ubx_nav_status);
00106 break;
00107 case MSG_SOL:
00108 _expect = sizeof(ubx_nav_solution);
00109 break;
00110 case MSG_VELNED:
00111 _expect = sizeof(ubx_nav_velned);
00112 break;
00113 default:
00114 _gather = false;
00115 }
00116 }
00117 break;
00118 case 4:
00119 _step++;
00120 _ck_b += (_ck_a += data);
00121 _payload_length = data;
00122 break;
00123 case 5:
00124 _step++;
00125 _ck_b += (_ck_a += data);
00126 _payload_length += (uint16_t)data;
00127 _payload_counter = 0;
00128 if (_payload_length != _expect)
00129 _gather = false;
00130 break;
00131
00132
00133
00134 case 6:
00135 _ck_b += (_ck_a += data);
00136 if (_gather)
00137 _buffer.bytes[_payload_counter] = data;
00138 if (++_payload_counter == _payload_length)
00139 _step++;
00140 break;
00141
00142
00143
00144 case 7:
00145 _step++;
00146 if (_ck_a != data)
00147 _step = 0;
00148 break;
00149 case 8:
00150 _step = 0;
00151 if (_ck_b != data)
00152 break;
00153
00154 if (_gather) {
00155 parsed = _parse_gps();
00156 }
00157 }
00158 }
00159 return parsed;
00160 }
00161
00162
00163
00164 bool
00165 AP_GPS_UBLOX::_parse_gps(void)
00166 {
00167 switch (_msg_id) {
00168 case MSG_POSLLH:
00169 time = _buffer.posllh.time;
00170 longitude = _buffer.posllh.longitude;
00171 latitude = _buffer.posllh.latitude;
00172 altitude = _buffer.posllh.altitude_msl / 10;
00173 break;
00174 case MSG_STATUS:
00175 fix = (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.status.fix_type == FIX_3D);
00176 break;
00177 case MSG_SOL:
00178 fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D);
00179 num_sats = _buffer.solution.satellites;
00180 break;
00181 case MSG_VELNED:
00182 speed_3d = _buffer.velned.speed_3d;
00183 ground_speed = _buffer.velned.speed_2d;
00184 ground_course = _buffer.velned.heading_2d / 1000;
00185 break;
00186 default:
00187 return false;
00188 }
00189 return true;
00190 }