Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

uncrustify libraries/AP_GPS/AP_GPS_MTK16.cpp

This commit is contained in:
uncrustify 2012-08-21 19:19:52 -07:00 committed by Pat Hickey
parent 87299da0dd
commit 7de08acf09
1 changed files with 55 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

110
libraries/AP_GPS/AP_GPS_MTK16.cpp
View File

@ -14,9 +14,9 @@
#include "AP_GPS_MTK16.h" #include "AP_GPS_MTK16.h"
#include <stdint.h> #include <stdint.h>
#if defined(ARDUINO) && ARDUINO >= 100 #if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h" #include "Arduino.h"
#else #else
#include <wiring.h> #include <wiring.h>
#endif #endif
// Constructors //////////////////////////////////////////////////////////////// // Constructors ////////////////////////////////////////////////////////////////
@ -37,12 +37,12 @@ AP_GPS_MTK16::init(enum GPS_Engine_Setting nav_setting)
// set 5Hz update rate // set 5Hz update rate
_port->print(MTK_OUTPUT_5HZ); _port->print(MTK_OUTPUT_5HZ);
// set SBAS on // set SBAS on
_port->print(SBAS_ON); _port->print(SBAS_ON);
// set WAAS on // set WAAS on
_port->print(WAAS_ON); _port->print(WAAS_ON);
// set initial epoch code // set initial epoch code
_epoch = TIME_OF_DAY; _epoch = TIME_OF_DAY;
_time_offset = 0; _time_offset = 0;
@ -64,12 +64,12 @@ AP_GPS_MTK16::init(enum GPS_Engine_Setting nav_setting)
bool bool
AP_GPS_MTK16::read(void) AP_GPS_MTK16::read(void)
{ {
uint8_t data; uint8_t data;
int16_t numc; int16_t numc;
bool parsed = false; bool parsed = false;
numc = _port->available(); numc = _port->available();
for (int16_t i = 0; i < numc; i++) { // Process bytes received for (int16_t i = 0; i < numc; i++) { // Process bytes received
// read the next byte // read the next byte
data = _port->read(); data = _port->read();
@ -77,15 +77,15 @@ AP_GPS_MTK16::read(void)
restart: restart:
switch(_step) { switch(_step) {
// Message preamble, class, ID detection // Message preamble, class, ID detection
// //
// If we fail to match any of the expected bytes, we // If we fail to match any of the expected bytes, we
// reset the state machine and re-consider the failed // reset the state machine and re-consider the failed
// byte as the first byte of the preamble. This // byte as the first byte of the preamble. This
// improves our chances of recovering from a mismatch // improves our chances of recovering from a mismatch
// and makes it less likely that we will be fooled by // and makes it less likely that we will be fooled by
// the preamble appearing as data in some other message. // the preamble appearing as data in some other message.
// //
case 0: case 0:
if(PREAMBLE1 == data) if(PREAMBLE1 == data)
_step++; _step++;
@ -100,16 +100,16 @@ restart:
case 2: case 2:
if (sizeof(_buffer) == data) { if (sizeof(_buffer) == data) {
_step++; _step++;
_ck_b = _ck_a = data; // reset the checksum accumulators _ck_b = _ck_a = data; // reset the checksum accumulators
_payload_counter = 0; _payload_counter = 0;
} else { } 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; goto restart;
} }
break; break;
// Receive message data // Receive message data
// //
case 3: case 3:
_buffer.bytes[_payload_counter++] = data; _buffer.bytes[_payload_counter++] = data;
_ck_b += (_ck_a += data); _ck_b += (_ck_a += data);
@ -117,8 +117,8 @@ restart:
_step++; _step++;
break; break;
// Checksum and message processing // Checksum and message processing
// //
case 4: case 4:
_step++; _step++;
if (_ck_a != data) { if (_ck_a != data) {
@ -131,19 +131,19 @@ restart:
break; break;
} }
fix = ((_buffer.msg.fix_type == FIX_3D) || fix = ((_buffer.msg.fix_type == FIX_3D) ||
(_buffer.msg.fix_type == FIX_3D_SBAS)); (_buffer.msg.fix_type == FIX_3D_SBAS));
latitude = _buffer.msg.latitude * 10; // XXX doc says *10e7 but device says otherwise latitude = _buffer.msg.latitude * 10; // XXX doc says *10e7 but device says otherwise
longitude = _buffer.msg.longitude * 10; // XXX doc says *10e7 but device says otherwise longitude = _buffer.msg.longitude * 10; // XXX doc says *10e7 but device says otherwise
altitude = _buffer.msg.altitude; altitude = _buffer.msg.altitude;
ground_speed = _buffer.msg.ground_speed; ground_speed = _buffer.msg.ground_speed;
ground_course = _buffer.msg.ground_course; ground_course = _buffer.msg.ground_course;
num_sats = _buffer.msg.satellites; num_sats = _buffer.msg.satellites;
hdop = _buffer.msg.hdop; hdop = _buffer.msg.hdop;
date = _buffer.msg.utc_date; date = _buffer.msg.utc_date;
// time from gps is UTC, but convert here to msToD // time from gps is UTC, but convert here to msToD
int32_t time_utc = _buffer.msg.utc_time; int32_t time_utc = _buffer.msg.utc_time;
int32_t temp = (time_utc/10000000); int32_t temp = (time_utc/10000000);
time_utc -= temp*10000000; time_utc -= temp*10000000;
time = temp * 3600000; time = temp * 3600000;
@ -154,26 +154,26 @@ restart:
parsed = true; parsed = true;
#ifdef FAKE_GPS_LOCK_TIME #ifdef FAKE_GPS_LOCK_TIME
if (millis() > FAKE_GPS_LOCK_TIME*1000) { if (millis() > FAKE_GPS_LOCK_TIME*1000) {
fix = true; fix = true;
latitude = -35000000UL; latitude = -35000000UL;
longitude = 149000000UL; longitude = 149000000UL;
altitude = 584; altitude = 584;
} }
#endif #endif
/* Waiting on clarification of MAVLink protocol! /* Waiting on clarification of MAVLink protocol!
if(!_offset_calculated && parsed) { * if(!_offset_calculated && parsed) {
int32_t tempd1 = date; * int32_t tempd1 = date;
int32_t day = tempd1/10000; * int32_t day = tempd1/10000;
tempd1 -= day * 10000; * tempd1 -= day * 10000;
int32_t month = tempd1/100; * int32_t month = tempd1/100;
int32_t year = tempd1 - month * 100; * int32_t year = tempd1 - month * 100;
_time_offset = _calc_epoch_offset(day, month, year); * _time_offset = _calc_epoch_offset(day, month, year);
_epoch = UNIX_EPOCH; * _epoch = UNIX_EPOCH;
_offset_calculated = TRUE; * _offset_calculated = TRUE;
} * }
*/ */
} }
} }