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
edb627a5
Commit
edb627a5
authored
13 years ago
by
Andrew Tridgell
Browse files
Options
Downloads
Patches
Plain Diff
AP_Declination: added timing information to declination test
parent
01c4cde1
No related branches found
Branches containing commit
No related tags found
No related merge requests found
Changes
1
Hide whitespace changes
Inline
Side-by-side
Showing
1 changed file
libraries/AP_Declination/examples/AP_Declination_test/AP_Declination_test.pde
+33
-25
33 additions, 25 deletions
...tion/examples/AP_Declination_test/AP_Declination_test.pde
with
33 additions
and
25 deletions
libraries/AP_Declination/examples/AP_Declination_test/AP_Declination_test.pde
+
33
−
25
View file @
edb627a5
...
@@ -6,6 +6,8 @@
...
@@ -6,6 +6,8 @@
#include
<AP_Math.h>
#include
<AP_Math.h>
#include
<AP_Declination.h>
#include
<AP_Declination.h>
#include
<Filter.h>
#include
<Filter.h>
#include
<I2C.h>
#include
<SPI.h>
#ifdef DESKTOP_BUILD
#ifdef DESKTOP_BUILD
// all of this is needed to build with SITL
// all of this is needed to build with SITL
...
@@ -66,10 +68,37 @@ static const int16_t dec_tbl[37][73] = \
...
@@ -66,10 +68,37 @@ static const int16_t dec_tbl[37][73] = \
{
168
,
173
,
178
,
176
,
171
,
166
,
161
,
156
,
151
,
146
,
141
,
136
,
131
,
126
,
121
,
116
,
111
,
106
,
101
,
-
96
,
-
91
,
-
86
,
-
81
,
-
76
,
-
71
,
-
66
,
-
61
,
-
56
,
-
51
,
-
46
,
-
41
,
-
36
,
-
31
,
-
26
,
-
21
,
-
16
,
-
11
,
-
6
,
-
1
,
3
,
8
,
13
,
18
,
23
,
28
,
33
,
38
,
43
,
48
,
53
,
58
,
63
,
68
,
73
,
78
,
83
,
88
,
93
,
98
,
103
,
108
,
113
,
118
,
123
,
128
,
133
,
138
,
143
,
148
,
153
,
158
,
163
,
168
},
\
{
168
,
173
,
178
,
176
,
171
,
166
,
161
,
156
,
151
,
146
,
141
,
136
,
131
,
126
,
121
,
116
,
111
,
106
,
101
,
-
96
,
-
91
,
-
86
,
-
81
,
-
76
,
-
71
,
-
66
,
-
61
,
-
56
,
-
51
,
-
46
,
-
41
,
-
36
,
-
31
,
-
26
,
-
21
,
-
16
,
-
11
,
-
6
,
-
1
,
3
,
8
,
13
,
18
,
23
,
28
,
33
,
38
,
43
,
48
,
53
,
58
,
63
,
68
,
73
,
78
,
83
,
88
,
93
,
98
,
103
,
108
,
113
,
118
,
123
,
128
,
133
,
138
,
143
,
148
,
153
,
158
,
163
,
168
},
\
};
};
static
float
get_declination
(
float
lat
,
float
lon
)
{
int16_t
decSW
,
decSE
,
decNW
,
decNE
,
lonmin
,
latmin
;
float
decmin
,
decmax
;
uint8_t
latmin_index
,
lonmin_index
;
// Validate input values
lat
=
constrain
(
lat
,
-
90
,
90
);
lon
=
constrain
(
lon
,
-
180
,
180
);
latmin
=
floor
(
lat
/
5
)
*
5
;
lonmin
=
floor
(
lon
/
5
)
*
5
;
latmin_index
=
(
90
+
latmin
)
/
5
;
lonmin_index
=
(
180
+
lonmin
)
/
5
;
decSW
=
dec_tbl
[
latmin_index
][
lonmin_index
];
decSE
=
dec_tbl
[
latmin_index
][
lonmin_index
+
1
];
decNE
=
dec_tbl
[
latmin_index
+
1
][
lonmin_index
+
1
];
decNW
=
dec_tbl
[
latmin_index
+
1
][
lonmin_index
];
decmin
=
(
lon
-
lonmin
)
/
5
*
(
decSE
-
decSW
)
+
decSW
;
decmax
=
(
lon
-
lonmin
)
/
5
*
(
decNE
-
decNW
)
+
decNW
;
return
(
lat
-
latmin
)
/
5
*
(
decmax
-
decmin
)
+
decmin
;
}
void
setup
(
void
)
void
setup
(
void
)
{
{
float
declination
,
declination_test
;
float
declination
,
declination_test
;
uint16_t
pass
=
0
,
fail
=
0
;
uint16_t
pass
=
0
,
fail
=
0
;
uint32_t
total_time
=
0
;
Serial
.
begin
(
115200
);
Serial
.
begin
(
115200
);
Serial
.
print
(
"Beginning Test. Please wait...
\n
"
);
Serial
.
print
(
"Beginning Test. Please wait...
\n
"
);
...
@@ -78,7 +107,9 @@ void setup(void)
...
@@ -78,7 +107,9 @@ void setup(void)
{
{
for
(
int16_t
j
=
-
180
;
j
<=
180
;
j
+=
5
)
for
(
int16_t
j
=
-
180
;
j
<=
180
;
j
+=
5
)
{
{
uint32_t
t1
=
micros
();
declination
=
AP_Declination
::
get_declination
(
i
,
j
);
declination
=
AP_Declination
::
get_declination
(
i
,
j
);
total_time
+=
micros
()
-
t1
;
declination_test
=
get_declination
(
i
,
j
);
declination_test
=
get_declination
(
i
,
j
);
if
(
declination
==
declination_test
)
if
(
declination
==
declination_test
)
{
{
...
@@ -95,34 +126,11 @@ void setup(void)
...
@@ -95,34 +126,11 @@ void setup(void)
Serial
.
print
(
"Ending Test.
\n\n
"
);
Serial
.
print
(
"Ending Test.
\n\n
"
);
Serial
.
printf
(
"Total Pass: %i
\n
"
,
pass
);
Serial
.
printf
(
"Total Pass: %i
\n
"
,
pass
);
Serial
.
printf
(
"Total Fail: %i
\n
"
,
fail
);
Serial
.
printf
(
"Total Fail: %i
\n
"
,
fail
);
Serial
.
printf
(
"Average time per call: %.1f usec
\n
"
,
total_time
/
(
float
)(
pass
+
fail
));
}
}
void
loop
(
void
)
void
loop
(
void
)
{
{
}
}
float
get_declination
(
float
lat
,
float
lon
)
{
int16_t
decSW
,
decSE
,
decNW
,
decNE
,
lonmin
,
latmin
;
float
decmin
,
decmax
;
uint8_t
latmin_index
,
lonmin_index
;
// Validate input values
lat
=
constrain
(
lat
,
-
90
,
90
);
lon
=
constrain
(
lon
,
-
180
,
180
);
latmin
=
floor
(
lat
/
5
)
*
5
;
lonmin
=
floor
(
lon
/
5
)
*
5
;
latmin_index
=
(
90
+
latmin
)
/
5
;
lonmin_index
=
(
180
+
lonmin
)
/
5
;
decSW
=
dec_tbl
[
latmin_index
][
lonmin_index
];
decSE
=
dec_tbl
[
latmin_index
][
lonmin_index
+
1
];
decNE
=
dec_tbl
[
latmin_index
+
1
][
lonmin_index
+
1
];
decNW
=
dec_tbl
[
latmin_index
+
1
][
lonmin_index
];
decmin
=
(
lon
-
lonmin
)
/
5
*
(
decSE
-
decSW
)
+
decSW
;
decmax
=
(
lon
-
lonmin
)
/
5
*
(
decNE
-
decNW
)
+
decNW
;
return
(
lat
-
latmin
)
/
5
*
(
decmax
-
decmin
)
+
decmin
;
}
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