Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012 #include "AP_GPS_SIRF.h"
00013 #include <stdint.h>
00014
00015
00016
00017
00018
00019
00020
00021 static uint8_t init_messages[] = {
00022 0xa0, 0xa2, 0x00, 0x08, 0xa6, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xa8, 0xb0, 0xb3,
00023 0xa0, 0xa2, 0x00, 0x08, 0xa6, 0x00, 0x29, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0xd0, 0xb0, 0xb3
00024 };
00025
00026
00027 AP_GPS_SIRF::AP_GPS_SIRF(Stream *s) : GPS(s)
00028 {
00029 }
00030
00031
00032 void
00033 AP_GPS_SIRF::init(void)
00034 {
00035 _port->flush();
00036
00037
00038
00039
00040
00041
00042 _port->write(init_messages, sizeof(init_messages));
00043 }
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054 bool
00055 AP_GPS_SIRF::read(void)
00056 {
00057 uint8_t data;
00058 int numc;
00059 bool parsed = false;
00060
00061 numc = _port->available();
00062 while(numc--) {
00063
00064
00065 data = _port->read();
00066
00067 switch(_step){
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078 case 1:
00079 if (PREAMBLE2 == data) {
00080 _step++;
00081 break;
00082 }
00083 _step = 0;
00084
00085 case 0:
00086 if(PREAMBLE1 == data)
00087 _step++;
00088 break;
00089
00090
00091
00092
00093
00094
00095 case 2:
00096 _step++;
00097 _payload_length = (uint16_t)data << 8;
00098 break;
00099 case 3:
00100 _step++;
00101 _payload_length |= data;
00102 _payload_counter = 0;
00103 _checksum = 0;
00104 break;
00105
00106
00107
00108
00109
00110
00111 case 4:
00112 _step++;
00113 _accumulate(data);
00114 _payload_length--;
00115 _gather = false;
00116 switch(data) {
00117 case MSG_GEONAV:
00118 if (_payload_length == sizeof(sirf_geonav)) {
00119 _gather = true;
00120 _msg_id = data;
00121 }
00122 break;
00123 }
00124 break;
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135 case 5:
00136 if (_gather) {
00137 _accumulate(data);
00138 _buffer.bytes[_payload_counter] = data;
00139 if (++_payload_counter == _payload_length)
00140 _step++;
00141 } else {
00142 if (++_payload_counter == _payload_length)
00143 _step = 0;
00144 }
00145 break;
00146
00147
00148
00149 case 6:
00150 _step++;
00151 if ((_checksum >> 8) != data) {
00152 _error("GPS_SIRF: checksum error\n");
00153 _step = 0;
00154 }
00155 break;
00156 case 7:
00157 _step = 0;
00158 if ((_checksum & 0xff) != data) {
00159 _error("GPS_SIRF: checksum error\n");
00160 break;
00161 }
00162 if (_gather) {
00163 parsed = _parse_gps();
00164 }
00165 }
00166 }
00167 return(parsed);
00168 }
00169
00170 bool
00171 AP_GPS_SIRF::_parse_gps(void)
00172 {
00173 switch(_msg_id) {
00174 case MSG_GEONAV:
00175 time = _swapl(&_buffer.nav.time);
00176
00177 fix = (0 == _buffer.nav.fix_invalid);
00178 latitude = _swapl(&_buffer.nav.latitude);
00179 longitude = _swapl(&_buffer.nav.longitude);
00180 altitude = _swapl(&_buffer.nav.altitude_msl);
00181 ground_speed = _swapi(&_buffer.nav.ground_speed);
00182
00183 if (ground_speed > 50)
00184 ground_course = _swapi(&_buffer.nav.ground_course);
00185 num_sats = _buffer.nav.satellites;
00186
00187 return true;
00188 }
00189 return false;
00190 }
00191
00192 void
00193 AP_GPS_SIRF::_accumulate(uint8_t val)
00194 {
00195 _checksum = (_checksum + val) & 0x7fff;
00196 }