Skip to content
GitLab
Explore
Sign in
Register
Primary navigation
Search or go to…
Project
B
Baitboat
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
Baitboat
Commits
c69bfa2c
Commit
c69bfa2c
authored
10 years ago
by
Randy Mackay
Browse files
Options
Downloads
Patches
Plain Diff
Tracker: limit pitch to PITCH_RANGE
Also set servo_limit flags for pitch
parent
f2dab172
No related branches found
Branches containing commit
No related tags found
Tags containing commit
No related merge requests found
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
AntennaTracker/AntennaTracker.pde
+10
-0
10 additions, 0 deletions
AntennaTracker/AntennaTracker.pde
AntennaTracker/servos.pde
+37
-9
37 additions, 9 deletions
AntennaTracker/servos.pde
with
47 additions
and
9 deletions
AntennaTracker/AntennaTracker.pde
+
10
−
0
View file @
c69bfa2c
...
...
@@ -237,6 +237,16 @@ static struct {
bool
scan_reverse_yaw
:
1
;
// controls direction of yaw movement in SCAN mode
}
nav_status
;
////////////////////////////////////////////////////////////////////////////////
// Servo state
////////////////////////////////////////////////////////////////////////////////
static
struct
{
bool
yaw_lower
:
1
;
// true if yaw servo has been limited from moving to a lower position (i.e. position or rate limited)
bool
yaw_upper
:
1
;
// true if yaw servo has been limited from moving to a higher position (i.e. position or rate limited)
bool
pitch_lower
:
1
;
// true if pitch servo has been limited from moving to a lower position (i.e. position or rate limited)
bool
pitch_upper
:
1
;
// true if pitch servo has been limited from moving to a higher position (i.e. position or rate limited)
}
servo_limit
;
/*
scheduler table - all regular tasks apart from the fast_loop()
should be listed here, along with how often they should be called
...
...
This diff is collapsed.
Click to expand it.
AntennaTracker/servos.pde
+
37
−
9
View file @
c69bfa2c
...
...
@@ -14,7 +14,8 @@ static void update_pitch_position_servo(float pitch)
// pitch argument is -90 to 90, where 0 is horizontal
// servo_out is in 100ths of a degree
float
ahrs_pitch
=
ahrs
.
pitch_sensor
*
0.01
f
;
int32_t
angle_err
=
(
ahrs_pitch
-
pitch
)
*
100.0
;
int32_t
angle_err
=
-
(
ahrs_pitch
-
pitch
)
*
100.0
;
int32_t
pitch_limit_cd
=
g
.
pitch_range
*
100
/
2
;
// Need to configure your servo so that increasing servo_out causes increase in pitch/elevation (ie pointing higher into the sky,
// above the horizon. On my antenna tracker this requires the pitch/elevation servo to be reversed
// param set RC2_REV -1
...
...
@@ -31,20 +32,40 @@ static void update_pitch_position_servo(float pitch)
// PITCH2SRV_I 0.020000
// PITCH2SRV_D 0.000000
// PITCH2SRV_IMAX 4000.000000
int32_t
new_servo_out
=
channel_pitch
.
servo_out
-
g
.
pidPitch2Srv
.
get_pid
(
angle_err
);
channel_pitch
.
servo_out
=
constrain_float
(
new_servo_out
,
-
9000
,
9000
);
// add slew rate limit
// calculate new servo position
int32_t
new_servo_out
=
channel_pitch
.
servo_out
+
g
.
pidPitch2Srv
.
get_pid
(
angle_err
);
// initialise limit flags
servo_limit
.
pitch_lower
=
false
;
servo_limit
.
pitch_upper
=
false
;
// rate limit pitch servo
if
(
g
.
pitch_slew_time
>
0.02
f
)
{
uint16_t
max_change
=
0.02
f
*
18000
/
g
.
pitch_slew_time
;
uint16_t
max_change
=
0.02
f
*
(
pitch_limit_cd
)
/
g
.
pitch_slew_time
;
if
(
max_change
<
1
)
{
max_change
=
1
;
}
new_servo_out
=
constrain_float
(
new_servo_out
,
channel_pitch
.
servo_out
-
max_change
,
channel_pitch
.
servo_out
+
max_change
);
if
(
new_servo_out
<=
channel_pitch
.
servo_out
-
max_change
)
{
new_servo_out
=
channel_pitch
.
servo_out
-
max_change
;
servo_limit
.
pitch_lower
=
true
;
}
if
(
new_servo_out
>=
channel_pitch
.
servo_out
+
max_change
)
{
new_servo_out
=
channel_pitch
.
servo_out
+
max_change
;
servo_limit
.
pitch_upper
=
true
;
}
}
channel_pitch
.
servo_out
=
new_servo_out
;
// position limit pitch servo
if
(
channel_pitch
.
servo_out
<=
-
pitch_limit_cd
)
{
channel_pitch
.
servo_out
=
-
pitch_limit_cd
;
servo_limit
.
pitch_lower
=
true
;
}
if
(
channel_pitch
.
servo_out
>=
pitch_limit_cd
)
{
channel_pitch
.
servo_out
=
pitch_limit_cd
;
servo_limit
.
pitch_upper
=
true
;
}
}
...
...
@@ -91,6 +112,8 @@ static void update_pitch_servo(float pitch)
update_pitch_position_servo
(
pitch
);
break
;
}
// convert servo_out to radio_out and send to servo
channel_pitch
.
calc_pwm
();
channel_pitch
.
output
();
}
...
...
@@ -158,7 +181,10 @@ static void update_yaw_position_servo(float yaw)
static
uint32_t
slew_start_ms
;
int8_t
new_slew_dir
=
slew_dir
;
// get earth frame z-axis rotation rate in radians
Vector3f
earth_rotation
=
ahrs
.
get_gyro
()
*
ahrs
.
get_dcm_matrix
();
bool
making_progress
;
if
(
slew_dir
!=
0
)
{
making_progress
=
(
-
slew_dir
*
earth_rotation
.
z
>=
0
);
...
...
@@ -207,7 +233,7 @@ static void update_yaw_position_servo(float yaw)
// add slew rate limit
if
(
g
.
yaw_slew_time
>
0.02
f
)
{
uint16_t
max_change
=
0.02
f
*
360
00.0
f
/
g
.
yaw_slew_time
;
uint16_t
max_change
=
0.02
f
*
1
00.0
f
*
g
.
yaw_range
/
g
.
yaw_slew_time
;
if
(
max_change
<
1
)
{
max_change
=
1
;
}
...
...
@@ -261,6 +287,8 @@ static void update_yaw_servo(float yaw)
update_yaw_position_servo
(
yaw
);
break
;
}
// convert servo_out to radio_out and send to servo
channel_yaw
.
calc_pwm
();
channel_yaw
.
output
();
}
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