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
84f82dd1
Commit
84f82dd1
authored
10 years ago
by
Randy Mackay
Browse files
Options
Downloads
Patches
Plain Diff
Copter: correct get_pilot_desired_throttle calc
Also minor format and commenting addition
parent
73f3b50e
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
+2
-2
2 additions, 2 deletions
ArduCopter/Attitude.pde
with
2 additions
and
2 deletions
ArduCopter/Attitude.pde
+
2
−
2
View file @
84f82dd1
...
@@ -140,7 +140,7 @@ static int16_t get_pilot_desired_throttle(int16_t throttle_control)
...
@@ -140,7 +140,7 @@ static int16_t get_pilot_desired_throttle(int16_t throttle_control)
throttle_out
=
g
.
throttle_min
+
((
float
)(
throttle_control
-
g
.
throttle_min
))
*
((
float
)(
g
.
throttle_mid
-
g
.
throttle_min
))
/
((
float
)(
mid_stick
-
g
.
throttle_min
));
throttle_out
=
g
.
throttle_min
+
((
float
)(
throttle_control
-
g
.
throttle_min
))
*
((
float
)(
g
.
throttle_mid
-
g
.
throttle_min
))
/
((
float
)(
mid_stick
-
g
.
throttle_min
));
}
else
if
(
throttle_control
>
mid_stick
)
{
}
else
if
(
throttle_control
>
mid_stick
)
{
// above the deadband
// above the deadband
throttle_out
=
g
.
throttle_mid
+
((
float
)(
throttle_control
-
mid_stick
))
*
(
float
)(
1000
-
g
.
throttle_mid
)
/
mid_stick
;
throttle_out
=
g
.
throttle_mid
+
((
float
)(
throttle_control
-
mid_stick
))
*
(
float
)(
1000
-
g
.
throttle_mid
)
/
(
float
)(
1000
-
mid_stick
)
;
}
else
{
}
else
{
// must be in the deadband
// must be in the deadband
throttle_out
=
g
.
throttle_mid
;
throttle_out
=
g
.
throttle_mid
;
...
@@ -165,7 +165,6 @@ static int16_t get_pilot_desired_climb_rate(int16_t throttle_control)
...
@@ -165,7 +165,6 @@ static int16_t get_pilot_desired_climb_rate(int16_t throttle_control)
int16_t
deadband_top
=
mid_stick
+
g
.
throttle_deadzone
;
int16_t
deadband_top
=
mid_stick
+
g
.
throttle_deadzone
;
int16_t
deadband_bottom
=
mid_stick
-
g
.
throttle_deadzone
;
int16_t
deadband_bottom
=
mid_stick
-
g
.
throttle_deadzone
;
// ensure a reasonable throttle value
// ensure a reasonable throttle value
throttle_control
=
constrain_int16
(
throttle_control
,
g
.
throttle_min
,
1000
);
throttle_control
=
constrain_int16
(
throttle_control
,
g
.
throttle_min
,
1000
);
...
@@ -208,6 +207,7 @@ static int16_t get_throttle_pre_takeoff(int16_t throttle_control)
...
@@ -208,6 +207,7 @@ static int16_t get_throttle_pre_takeoff(int16_t throttle_control)
return
0
;
return
0
;
}
}
// calculate mid stick and deadband
int16_t
mid_stick
=
g
.
rc_3
.
get_control_mid
();
int16_t
mid_stick
=
g
.
rc_3
.
get_control_mid
();
int16_t
deadband_top
=
mid_stick
+
g
.
throttle_deadzone
;
int16_t
deadband_top
=
mid_stick
+
g
.
throttle_deadzone
;
...
...
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