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
651a0182
Commit
651a0182
authored
10 years ago
by
Randy Mackay
Browse files
Options
Downloads
Patches
Plain Diff
Copter: move land_detector to separate file
parent
ca68a9a8
No related branches found
Branches containing commit
Tags
ArduCopter-2.1.0-Alpha
Tags containing commit
No related merge requests found
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
ArduCopter/control_land.pde
+0
-41
0 additions, 41 deletions
ArduCopter/control_land.pde
ArduCopter/land_detector.pde
+43
-0
43 additions, 0 deletions
ArduCopter/land_detector.pde
with
43 additions
and
41 deletions
ArduCopter/control_land.pde
+
0
−
41
View file @
651a0182
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// counter to verify landings
static
uint16_t
land_detector
=
LAND_DETECTOR_TRIGGER
;
// we assume we are landed
static
bool
land_with_gps
;
static
uint32_t
land_start_time
;
...
...
@@ -192,45 +190,6 @@ static float get_throttle_land()
}
}
// land_complete_maybe - return true if we may have landed (used to reset loiter targets during landing)
static
bool
land_complete_maybe
()
{
return
(
ap
.
land_complete
||
ap
.
land_complete_maybe
);
}
// update_land_detector - checks if we have landed and updates the ap.land_complete flag
// called at 50hz
static
void
update_land_detector
()
{
bool
climb_rate_small
=
abs
(
climb_rate
)
<
LAND_DETECTOR_CLIMBRATE_MAX
;
bool
target_climb_rate_low
=
!
pos_control
.
is_active_z
()
||
pos_control
.
get_desired_velocity
().
z
<
LAND_SPEED
;
bool
motor_at_lower_limit
=
motors
.
limit
.
throttle_lower
;
bool
throttle_low
=
FRAME_CONFIG
==
HELI_FRAME
||
motors
.
get_throttle_out
()
<
get_non_takeoff_throttle
();
bool
not_rotating_fast
=
ahrs
.
get_gyro
().
length
()
<
LAND_DETECTOR_ROTATION_MAX
;
if
(
climb_rate_small
&&
target_climb_rate_low
&&
motor_at_lower_limit
&&
throttle_low
&&
not_rotating_fast
)
{
if
(
!
ap
.
land_complete
)
{
// increase counter until we hit the trigger then set land complete flag
if
(
land_detector
<
LAND_DETECTOR_TRIGGER
)
{
land_detector
++
;
}
else
{
set_land_complete
(
true
);
land_detector
=
LAND_DETECTOR_TRIGGER
;
}
}
}
else
{
// we've sensed movement up or down so reset land_detector
land_detector
=
0
;
// if throttle output is high then clear landing flag
if
(
motors
.
get_throttle_out
()
>
get_non_takeoff_throttle
())
{
set_land_complete
(
false
);
}
}
// set land maybe flag
set_land_complete_maybe
(
land_detector
>=
LAND_DETECTOR_MAYBE_TRIGGER
);
}
// land_do_not_use_GPS - forces land-mode to not use the GPS but instead rely on pilot input for roll and pitch
// called during GPS failsafe to ensure that if we were already in LAND mode that we do not use the GPS
// has no effect if we are not already in LAND mode
...
...
This diff is collapsed.
Click to expand it.
ArduCopter/land_detector.pde
0 → 100644
+
43
−
0
View file @
651a0182
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// counter to verify landings
static
uint16_t
land_detector
=
LAND_DETECTOR_TRIGGER
;
// we assume we are landed
// land_complete_maybe - return true if we may have landed (used to reset loiter targets during landing)
static
bool
land_complete_maybe
()
{
return
(
ap
.
land_complete
||
ap
.
land_complete_maybe
);
}
// update_land_detector - checks if we have landed and updates the ap.land_complete flag
// called at 50hz
static
void
update_land_detector
()
{
bool
climb_rate_small
=
abs
(
climb_rate
)
<
LAND_DETECTOR_CLIMBRATE_MAX
;
bool
target_climb_rate_low
=
!
pos_control
.
is_active_z
()
||
pos_control
.
get_desired_velocity
().
z
<
LAND_SPEED
;
bool
motor_at_lower_limit
=
motors
.
limit
.
throttle_lower
;
bool
throttle_low
=
FRAME_CONFIG
==
HELI_FRAME
||
motors
.
get_throttle_out
()
<
get_non_takeoff_throttle
();
bool
not_rotating_fast
=
ahrs
.
get_gyro
().
length
()
<
LAND_DETECTOR_ROTATION_MAX
;
if
(
climb_rate_small
&&
target_climb_rate_low
&&
motor_at_lower_limit
&&
throttle_low
&&
not_rotating_fast
)
{
if
(
!
ap
.
land_complete
)
{
// increase counter until we hit the trigger then set land complete flag
if
(
land_detector
<
LAND_DETECTOR_TRIGGER
)
{
land_detector
++
;
}
else
{
set_land_complete
(
true
);
land_detector
=
LAND_DETECTOR_TRIGGER
;
}
}
}
else
{
// we've sensed movement up or down so reset land_detector
land_detector
=
0
;
// if throttle output is high then clear landing flag
if
(
motors
.
get_throttle_out
()
>
get_non_takeoff_throttle
())
{
set_land_complete
(
false
);
}
}
// set land maybe flag
set_land_complete_maybe
(
land_detector
>=
LAND_DETECTOR_MAYBE_TRIGGER
);
}
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