Skip to content
GitLab
Explore
Sign in
Register
Primary navigation
Search or go to…
Project
A
Ardupilot
Manage
Activity
Members
Labels
Plan
Issues
0
Issue boards
Milestones
Wiki
Code
Merge requests
0
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Model registry
Operate
Environments
Monitor
Incidents
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
OpenSource
Ardupilot
Commits
924dea9a
Commit
924dea9a
authored
12 years ago
by
uncrustify
Committed by
Pat Hickey
12 years ago
Browse files
Options
Downloads
Patches
Plain Diff
uncrustify libraries/AP_GPS/AP_GPS_SIRF.cpp
parent
f10307bc
No related branches found
Branches containing commit
No related tags found
Tags containing commit
No related merge requests found
Changes
1
Hide whitespace changes
Inline
Side-by-side
Showing
1 changed file
libraries/AP_GPS/AP_GPS_SIRF.cpp
+45
-45
45 additions, 45 deletions
libraries/AP_GPS/AP_GPS_SIRF.cpp
with
45 additions
and
45 deletions
libraries/AP_GPS/AP_GPS_SIRF.cpp
+
45
−
45
View file @
924dea9a
...
@@ -18,7 +18,7 @@
...
@@ -18,7 +18,7 @@
//
//
// XXX the bytes show up on the wire, but at least my test unit (EM-411) seems to ignore them.
// XXX the bytes show up on the wire, but at least my test unit (EM-411) seems to ignore them.
//
//
static
uint8_t
init_messages
[]
=
{
static
uint8_t
init_messages
[]
=
{
0xa0
,
0xa2
,
0x00
,
0x08
,
0xa6
,
0x02
,
0x00
,
0x00
,
0x00
,
0x00
,
0x00
,
0x00
,
0x00
,
0xa8
,
0xb0
,
0xb3
,
0xa0
,
0xa2
,
0x00
,
0x08
,
0xa6
,
0x02
,
0x00
,
0x00
,
0x00
,
0x00
,
0x00
,
0x00
,
0x00
,
0xa8
,
0xb0
,
0xb3
,
0xa0
,
0xa2
,
0x00
,
0x08
,
0xa6
,
0x00
,
0x29
,
0x01
,
0x00
,
0x00
,
0x00
,
0x00
,
0x00
,
0xd0
,
0xb0
,
0xb3
0xa0
,
0xa2
,
0x00
,
0x08
,
0xa6
,
0x00
,
0x29
,
0x01
,
0x00
,
0x00
,
0x00
,
0x00
,
0x00
,
0xd0
,
0xb0
,
0xb3
};
};
...
@@ -55,9 +55,9 @@ AP_GPS_SIRF::init(enum GPS_Engine_Setting nav_setting)
...
@@ -55,9 +55,9 @@ AP_GPS_SIRF::init(enum GPS_Engine_Setting nav_setting)
bool
bool
AP_GPS_SIRF
::
read
(
void
)
AP_GPS_SIRF
::
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
();
while
(
numc
--
)
{
while
(
numc
--
)
{
...
@@ -67,32 +67,32 @@ AP_GPS_SIRF::read(void)
...
@@ -67,32 +67,32 @@ AP_GPS_SIRF::read(void)
switch
(
_step
)
{
switch
(
_step
)
{
// Message preamble detection
// Message preamble detection
//
//
// If we fail to match any of the expected bytes, we reset
// If we fail to match any of the expected bytes, we reset
// the state machine and re-consider the failed byte as
// the state machine and re-consider the failed byte as
// the first byte of the preamble. This improves our
// the first byte of the preamble. This improves our
// chances of recovering from a mismatch and makes it less
// chances of recovering from a mismatch and makes it less
// likely that we will be fooled by the preamble appearing
// likely that we will be fooled by the preamble appearing
// as data in some other message.
// as data in some other message.
//
//
case
1
:
case
1
:
if
(
PREAMBLE2
==
data
)
{
if
(
PREAMBLE2
==
data
)
{
_step
++
;
_step
++
;
break
;
break
;
}
}
_step
=
0
;
_step
=
0
;
// FALLTHROUGH
// FALLTHROUGH
case
0
:
case
0
:
if
(
PREAMBLE1
==
data
)
if
(
PREAMBLE1
==
data
)
_step
++
;
_step
++
;
break
;
break
;
// Message length
// Message length
//
//
// We always collect the length so that we can avoid being
// We always collect the length so that we can avoid being
// fooled by preamble bytes in messages.
// fooled by preamble bytes in messages.
//
//
case
2
:
case
2
:
_step
++
;
_step
++
;
_payload_length
=
(
uint16_t
)
data
<<
8
;
_payload_length
=
(
uint16_t
)
data
<<
8
;
...
@@ -104,11 +104,11 @@ AP_GPS_SIRF::read(void)
...
@@ -104,11 +104,11 @@ AP_GPS_SIRF::read(void)
_checksum
=
0
;
_checksum
=
0
;
break
;
break
;
// Message header processing
// Message header processing
//
//
// We sniff the message ID to determine whether we are going
// We sniff the message ID to determine whether we are going
// to gather the message bytes or just discard them.
// to gather the message bytes or just discard them.
//
//
case
4
:
case
4
:
_step
++
;
_step
++
;
_accumulate
(
data
);
_accumulate
(
data
);
...
@@ -124,17 +124,17 @@ AP_GPS_SIRF::read(void)
...
@@ -124,17 +124,17 @@ AP_GPS_SIRF::read(void)
}
}
break
;
break
;
// Receive message data
// Receive message data
//
//
// Note that we are effectively guaranteed by the protocol
// Note that we are effectively guaranteed by the protocol
// that the checksum and postamble cannot be mistaken for
// that the checksum and postamble cannot be mistaken for
// the preamble, so if we are discarding bytes in this
// the preamble, so if we are discarding bytes in this
// message when the payload is done we return directly
// message when the payload is done we return directly
// to the preamble detector rather than bothering with
// to the preamble detector rather than bothering with
// the checksum logic.
// the checksum logic.
//
//
case
5
:
case
5
:
if
(
_gather
)
{
// gather data if requested
if
(
_gather
)
{
// gather data if requested
_accumulate
(
data
);
_accumulate
(
data
);
_buffer
.
bytes
[
_payload_counter
]
=
data
;
_buffer
.
bytes
[
_payload_counter
]
=
data
;
if
(
++
_payload_counter
==
_payload_length
)
if
(
++
_payload_counter
==
_payload_length
)
...
@@ -145,8 +145,8 @@ AP_GPS_SIRF::read(void)
...
@@ -145,8 +145,8 @@ AP_GPS_SIRF::read(void)
}
}
break
;
break
;
// Checksum and message processing
// Checksum and message processing
//
//
case
6
:
case
6
:
_step
++
;
_step
++
;
if
((
_checksum
>>
8
)
!=
data
)
{
if
((
_checksum
>>
8
)
!=
data
)
{
...
@@ -161,7 +161,7 @@ AP_GPS_SIRF::read(void)
...
@@ -161,7 +161,7 @@ AP_GPS_SIRF::read(void)
break
;
break
;
}
}
if
(
_gather
)
{
if
(
_gather
)
{
parsed
=
_parse_gps
();
// Parse the new GPS packet
parsed
=
_parse_gps
();
// Parse the new GPS packet
}
}
}
}
}
}
...
@@ -173,17 +173,17 @@ AP_GPS_SIRF::_parse_gps(void)
...
@@ -173,17 +173,17 @@ AP_GPS_SIRF::_parse_gps(void)
{
{
switch
(
_msg_id
)
{
switch
(
_msg_id
)
{
case
MSG_GEONAV
:
case
MSG_GEONAV
:
time
=
_swapl
(
&
_buffer
.
nav
.
time
);
time
=
_swapl
(
&
_buffer
.
nav
.
time
);
//fix = (0 == _buffer.nav.fix_invalid) && (FIX_3D == (_buffer.nav.fix_type & FIX_MASK));
//fix = (0 == _buffer.nav.fix_invalid) && (FIX_3D == (_buffer.nav.fix_type & FIX_MASK));
fix
=
(
0
==
_buffer
.
nav
.
fix_invalid
);
fix
=
(
0
==
_buffer
.
nav
.
fix_invalid
);
latitude
=
_swapl
(
&
_buffer
.
nav
.
latitude
);
latitude
=
_swapl
(
&
_buffer
.
nav
.
latitude
);
longitude
=
_swapl
(
&
_buffer
.
nav
.
longitude
);
longitude
=
_swapl
(
&
_buffer
.
nav
.
longitude
);
altitude
=
_swapl
(
&
_buffer
.
nav
.
altitude_msl
);
altitude
=
_swapl
(
&
_buffer
.
nav
.
altitude_msl
);
ground_speed
=
_swapi
(
&
_buffer
.
nav
.
ground_speed
);
ground_speed
=
_swapi
(
&
_buffer
.
nav
.
ground_speed
);
// at low speeds, ground course wanders wildly; suppress changes if we are not moving
// at low speeds, ground course wanders wildly; suppress changes if we are not moving
if
(
ground_speed
>
50
)
if
(
ground_speed
>
50
)
ground_course
=
_swapi
(
&
_buffer
.
nav
.
ground_course
);
ground_course
=
_swapi
(
&
_buffer
.
nav
.
ground_course
);
num_sats
=
_buffer
.
nav
.
satellites
;
num_sats
=
_buffer
.
nav
.
satellites
;
return
true
;
return
true
;
}
}
...
...
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment