diff --git a/libraries/AP_GPS/AP_GPS_MTK.cpp b/libraries/AP_GPS/AP_GPS_MTK.cpp index 82e5aa75e1..039b33e066 100644 --- a/libraries/AP_GPS/AP_GPS_MTK.cpp +++ b/libraries/AP_GPS/AP_GPS_MTK.cpp @@ -50,7 +50,7 @@ void AP_GPS_MTK::update(void) data = _port->read(); restart: - switch(step){ + switch(_step){ // Message preamble, class, ID detection // @@ -63,32 +63,32 @@ restart: // case 0: if(PREAMBLE1 == data) - step++; + _step++; break; case 1: if (PREAMBLE2 == data) { - step++; + _step++; break; } - step = 0; + _step = 0; goto restart; case 2: if (MESSAGE_CLASS == data) { - step++; - ck_b = ck_a = data; // reset the checksum accumulators + _step++; + _ck_b = _ck_a = data; // reset the checksum accumulators } else { - step = 0; // reset and wait for a message of the right class + _step = 0; // reset and wait for a message of the right class goto restart; } break; case 3: if (MESSAGE_ID == data) { - step++; - ck_b += (ck_a += data); - payload_length = sizeof(buffer); // prepare to receive our message - payload_counter = 0; + _step++; + _ck_b += (_ck_a += data); + _payload_length = sizeof(diyd_mtk_msg); // prepare to receive our message + _payload_counter = 0; } else { - step = 0; + _step = 0; goto restart; } break; @@ -96,24 +96,24 @@ restart: // Receive message data // case 4: - buffer.bytes[payload_counter++] = data; - ck_b += (ck_a += data); - if (payload_counter == payload_length) - step++; + _buffer.bytes[_payload_counter++] = data; + _ck_b += (_ck_a += data); + if (_payload_counter == _payload_length) + _step++; break; // Checksum and message processing // case 5: - step++; - if (ck_a != data) { + _step++; + if (_ck_a != data) { _error("GPS_MTK: checksum error\n"); - step = 0; + _step = 0; } break; case 6: - step = 0; - if (ck_b != data) { + _step = 0; + if (_ck_b != data) { _error("GPS_MTK: checksum error\n"); break; } @@ -122,24 +122,23 @@ restart: } } - // Private Methods void AP_GPS_MTK::_parse_gps(void) { - if (FIX_3D != buffer.msg.fix_type) { + if (FIX_3D != _buffer.msg.fix_type) { fix = false; } else { fix = true; - latitude = _swapl(&buffer.msg.latitude) * 10; - longitude = _swapl(&buffer.msg.longitude) * 10; - altitude = _swapl(&buffer.msg.altitude); - ground_speed = _swapl(&buffer.msg.ground_speed); - ground_course = _swapl(&buffer.msg.ground_course) / 10000; - num_sats = buffer.msg.satellites; + latitude = _swapl(&_buffer.msg.latitude) * 10; + longitude = _swapl(&_buffer.msg.longitude) * 10; + altitude = _swapl(&_buffer.msg.altitude); + ground_speed = _swapl(&_buffer.msg.ground_speed); + ground_course = _swapl(&_buffer.msg.ground_course) / 10000; + num_sats = _buffer.msg.satellites; // XXX docs say this is UTC, but our clients expect msToW - time = _swapl(&buffer.msg.utc_time); + time = _swapl(&_buffer.msg.utc_time); } new_data = true; } diff --git a/libraries/AP_GPS/AP_GPS_MTK.h b/libraries/AP_GPS/AP_GPS_MTK.h index cd0a42ac7f..5f032b62a6 100644 --- a/libraries/AP_GPS/AP_GPS_MTK.h +++ b/libraries/AP_GPS/AP_GPS_MTK.h @@ -66,19 +66,19 @@ private: }; // Packet checksum accumulators - uint8_t ck_a; - uint8_t ck_b; + uint8_t _ck_a; + uint8_t _ck_b; // State machine state - uint8_t step; - uint8_t payload_length; - uint8_t payload_counter; + uint8_t _step; + uint8_t _payload_length; + uint8_t _payload_counter; // Receive buffer union { diyd_mtk_msg msg; uint8_t bytes[]; - } buffer; + } _buffer; // Buffer parse & GPS state update void _parse_gps();