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
f6e12bda
Commit
f6e12bda
authored
10 years ago
by
Randy Mackay
Browse files
Options
Downloads
Patches
Plain Diff
AC_AttControlHeli: integrate div-by-zero check for bf-to-ef conversion
parent
c45338f0
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
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
+5
-4
5 additions, 4 deletions
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
with
5 additions
and
4 deletions
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
+
5
−
4
View file @
f6e12bda
...
@@ -91,10 +91,11 @@ void AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_yaw(float roll_pass
...
@@ -91,10 +91,11 @@ void AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_yaw(float roll_pass
// update our earth-frame angle targets
// update our earth-frame angle targets
Vector3f
angle_ef_error
;
Vector3f
angle_ef_error
;
frame_conversion_bf_to_ef
(
_angle_bf_error
,
angle_ef_error
);
if
(
frame_conversion_bf_to_ef
(
_angle_bf_error
,
angle_ef_error
))
{
_angle_ef_target
.
x
=
wrap_180_cd_float
(
angle_ef_error
.
x
+
_ahrs
.
roll_sensor
);
_angle_ef_target
.
x
=
wrap_180_cd_float
(
angle_ef_error
.
x
+
_ahrs
.
roll_sensor
);
_angle_ef_target
.
y
=
wrap_180_cd_float
(
angle_ef_error
.
y
+
_ahrs
.
pitch_sensor
);
_angle_ef_target
.
y
=
wrap_180_cd_float
(
angle_ef_error
.
y
+
_ahrs
.
pitch_sensor
);
_angle_ef_target
.
z
=
wrap_360_cd_float
(
angle_ef_error
.
z
+
_ahrs
.
yaw_sensor
);
_angle_ef_target
.
z
=
wrap_360_cd_float
(
angle_ef_error
.
z
+
_ahrs
.
yaw_sensor
);
}
// handle flipping over pitch axis
// handle flipping over pitch axis
if
(
_angle_ef_target
.
y
>
9000.0
f
)
{
if
(
_angle_ef_target
.
y
>
9000.0
f
)
{
...
...
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