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
69134002
Commit
69134002
authored
13 years ago
by
Jason Short
Browse files
Options
Downloads
Patches
Plain Diff
cast D term to float just in case
parent
43e695b1
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
ArduCopter/Attitude.pde
+5
-3
5 additions, 3 deletions
ArduCopter/Attitude.pde
with
5 additions
and
3 deletions
ArduCopter/Attitude.pde
+
5
−
3
View file @
69134002
...
...
@@ -21,7 +21,7 @@ get_stabilize_roll(int32_t target_angle)
// limit the error we're feeding to the PID
target_angle
=
constrain
(
target_angle
,
-
2500
,
2500
);
// conver to desired Rate:
// conver
t
to desired Rate:
int32_t
target_rate
=
g
.
pi_stabilize_roll
.
get_p
(
target_angle
);
int16_t
iterm
=
g
.
pi_stabilize_roll
.
get_i
(
target_angle
,
G_Dt
);
...
...
@@ -115,7 +115,8 @@ get_rate_roll(int32_t target_rate)
target_rate
=
g
.
pid_rate_roll
.
get_pid
(
target_rate
,
G_Dt
);
// Dampening
target_rate
-=
constrain
((
current_rate
-
last_rate
)
*
g
.
stabilize_d
,
-
500
,
500
);
int16_t
d_temp
=
(
float
)(
current_rate
-
last_rate
)
*
g
.
stabilize_d
;
target_rate
-=
constrain
(
d_temp
,
-
500
,
500
);
last_rate
=
current_rate
;
// output control:
...
...
@@ -133,7 +134,8 @@ get_rate_pitch(int32_t target_rate)
target_rate
=
g
.
pid_rate_pitch
.
get_pid
(
target_rate
,
G_Dt
);
// Dampening
target_rate
-=
constrain
((
current_rate
-
last_rate
)
*
g
.
stabilize_d
,
-
500
,
500
);
int16_t
d_temp
=
(
float
)(
current_rate
-
last_rate
)
*
g
.
stabilize_d
;
target_rate
-=
constrain
(
d_temp
,
-
500
,
500
);
last_rate
=
current_rate
;
// output control:
...
...
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