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
7afd443f
Commit
7afd443f
authored
10 years ago
by
Randy Mackay
Browse files
Options
Downloads
Patches
Plain Diff
Tracker: move bearing and dist calcs to separate function
parent
a0e89281
No related branches found
No related tags found
No related merge requests found
Changes
1
Hide whitespace changes
Inline
Side-by-side
Showing
1 changed file
AntennaTracker/tracking.pde
+29
-13
29 additions, 13 deletions
AntennaTracker/tracking.pde
with
29 additions
and
13 deletions
AntennaTracker/tracking.pde
+
29
−
13
View file @
7afd443f
...
...
@@ -354,6 +354,33 @@ static void update_vehicle_pos_estimate()
}
}
/**
update_bearing_and_distance - updates bearing and distance to the vehicle
should be called at 50hz
*/
static
void
update_bearing_and_distance
()
{
// exit immediately if we do not have a valid vehicle position
if
(
!
vehicle
.
location_valid
)
{
return
;
}
// calculate bearing to vehicle
// To-Do: remove need for check of control_mode
if
(
control_mode
!=
SCAN
&&
!
nav_status
.
manual_control_yaw
)
{
nav_status
.
bearing
=
get_bearing_cd
(
current_loc
,
vehicle
.
location_estimate
)
*
0.01
f
;
}
// calculate distance to vehicle
nav_status
.
distance
=
get_distance
(
current_loc
,
vehicle
.
location_estimate
);
// calculate pitch to vehicle
// To-Do: remove need for check of control_mode
if
(
control_mode
!=
SCAN
&&
!
nav_status
.
manual_control_pitch
)
{
nav_status
.
pitch
=
degrees
(
atan2f
(
nav_status
.
altitude_difference
,
nav_status
.
distance
));
}
}
/**
main antenna tracking code, called at 50Hz
*/
...
...
@@ -368,19 +395,8 @@ static void update_tracking(void)
current_loc
=
gps
.
location
();
}
// calculate the bearing to the vehicle
float
bearing
=
get_bearing_cd
(
current_loc
,
vehicle
.
location_estimate
)
*
0.01
f
;
float
distance
=
get_distance
(
current_loc
,
vehicle
.
location_estimate
);
float
pitch
=
degrees
(
atan2f
(
nav_status
.
altitude_difference
,
distance
));
// update nav_status for NAV_CONTROLLER_OUTPUT
if
(
control_mode
!=
SCAN
&&
!
nav_status
.
manual_control_yaw
)
{
nav_status
.
bearing
=
bearing
;
}
if
(
control_mode
!=
SCAN
&&
!
nav_status
.
manual_control_pitch
)
{
nav_status
.
pitch
=
pitch
;
}
nav_status
.
distance
=
distance
;
// update bearing and distance to vehicle
update_bearing_and_distance
();
switch
(
control_mode
)
{
case
AUTO
:
...
...
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