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
fc4b5c96
Commit
fc4b5c96
authored
11 years ago
by
Randy Mackay
Browse files
Options
Downloads
Patches
Plain Diff
Copter: pre-arm check that INAV has no errors
parent
e47e1f21
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/motors.pde
+21
-13
21 additions, 13 deletions
ArduCopter/motors.pde
with
21 additions
and
13 deletions
ArduCopter/motors.pde
+
21
−
13
View file @
fc4b5c96
...
@@ -125,6 +125,9 @@ static void init_arm_motors()
...
@@ -125,6 +125,9 @@ static void init_arm_motors()
// disable cpu failsafe because initialising everything takes a while
// disable cpu failsafe because initialising everything takes a while
failsafe_disable
();
failsafe_disable
();
// disable inertial nav errors temporarily
inertial_nav
.
ignore_next_error
();
#if LOGGING_ENABLED == ENABLED
#if LOGGING_ENABLED == ENABLED
// start dataflash
// start dataflash
start_logging
();
start_logging
();
...
@@ -289,19 +292,13 @@ static void pre_arm_checks(bool display_failure)
...
@@ -289,19 +292,13 @@ static void pre_arm_checks(bool display_failure)
// check GPS
// check GPS
if
((
g
.
arming_check_enabled
==
ARMING_CHECK_ALL
)
||
(
g
.
arming_check_enabled
&
ARMING_CHECK_GPS
))
{
if
((
g
.
arming_check_enabled
==
ARMING_CHECK_ALL
)
||
(
g
.
arming_check_enabled
&
ARMING_CHECK_GPS
))
{
// check gps is ok if required - note this same check is repeated again in arm_checks
// check gps is ok if required - note this same check is repeated again in arm_checks
if
((
mode_requires_GPS
(
control_mode
)
||
g
.
failsafe_gps_enabled
==
FS_GPS_LAND_EVEN_STABILIZE
)
&&
!
pre_arm_gps_checks
())
{
if
((
mode_requires_GPS
(
control_mode
)
||
g
.
failsafe_gps_enabled
==
FS_GPS_LAND_EVEN_STABILIZE
)
&&
!
pre_arm_gps_checks
(
display_failure
))
{
if
(
display_failure
)
{
gcs_send_text_P
(
SEVERITY_HIGH
,
PSTR
(
"PreArm: Bad GPS Pos"
));
}
return
;
return
;
}
}
#if AC_FENCE == ENABLED
#if AC_FENCE == ENABLED
// check fence is initialised
// check fence is initialised
if
(
!
fence
.
pre_arm_check
()
||
(((
fence
.
get_enabled_fences
()
&
AC_FENCE_TYPE_CIRCLE
)
!=
0
)
&&
!
pre_arm_gps_checks
()))
{
if
(
!
fence
.
pre_arm_check
()
||
(((
fence
.
get_enabled_fences
()
&
AC_FENCE_TYPE_CIRCLE
)
!=
0
)
&&
!
pre_arm_gps_checks
(
display_failure
)))
{
if
(
display_failure
)
{
gcs_send_text_P
(
SEVERITY_HIGH
,
PSTR
(
"PreArm: Bad GPS Pos"
));
}
return
;
return
;
}
}
#endif
#endif
...
@@ -415,12 +412,23 @@ static void pre_arm_rc_checks()
...
@@ -415,12 +412,23 @@ static void pre_arm_rc_checks()
}
}
// performs pre_arm gps related checks and returns true if passed
// performs pre_arm gps related checks and returns true if passed
static
bool
pre_arm_gps_checks
()
static
bool
pre_arm_gps_checks
(
bool
display_failure
)
{
{
float
speed_cms
=
inertial_nav
.
get_velocity
().
length
();
// speed according to inertial nav in cm/s
float
speed_cms
=
inertial_nav
.
get_velocity
().
length
();
// speed according to inertial nav in cm/s
// ensure GPS is ok and our speed is below 50cm/s
// ensure GPS is ok and our speed is below 50cm/s
if
(
!
GPS_ok
()
||
g_gps
->
hdop
>
g
.
gps_hdop_good
||
gps_glitch
.
glitching
()
||
speed_cms
==
0
||
speed_cms
>
PREARM_MAX_VELOCITY_CMS
)
{
if
(
!
GPS_ok
()
||
g_gps
->
hdop
>
g
.
gps_hdop_good
||
gps_glitch
.
glitching
()
||
speed_cms
==
0
||
speed_cms
>
PREARM_MAX_VELOCITY_CMS
)
{
if
(
display_failure
)
{
gcs_send_text_P
(
SEVERITY_HIGH
,
PSTR
(
"PreArm: Bad GPS Pos"
));
}
return
false
;
}
// check for missed gps updates (i.e. not arriving at at least 4hz)
if
(
inertial_nav
.
error_count
()
>
0
)
{
if
(
display_failure
)
{
gcs_send_text_P
(
SEVERITY_HIGH
,
PSTR
(
"PreArm: Slow GPS"
));
}
return
false
;
return
false
;
}
}
...
@@ -439,10 +447,7 @@ static bool arm_checks(bool display_failure)
...
@@ -439,10 +447,7 @@ static bool arm_checks(bool display_failure)
// check gps is ok if required - note this same check is also done in pre-arm checks
// check gps is ok if required - note this same check is also done in pre-arm checks
if
((
g
.
arming_check_enabled
==
ARMING_CHECK_ALL
)
||
(
g
.
arming_check_enabled
&
ARMING_CHECK_GPS
))
{
if
((
g
.
arming_check_enabled
==
ARMING_CHECK_ALL
)
||
(
g
.
arming_check_enabled
&
ARMING_CHECK_GPS
))
{
if
((
mode_requires_GPS
(
control_mode
)
||
g
.
failsafe_gps_enabled
==
FS_GPS_LAND_EVEN_STABILIZE
)
&&
!
pre_arm_gps_checks
())
{
if
((
mode_requires_GPS
(
control_mode
)
||
g
.
failsafe_gps_enabled
==
FS_GPS_LAND_EVEN_STABILIZE
)
&&
!
pre_arm_gps_checks
(
display_failure
))
{
if
(
display_failure
)
{
gcs_send_text_P
(
SEVERITY_HIGH
,
PSTR
(
"Arm: Bad GPS Pos"
));
}
return
false
;
return
false
;
}
}
}
}
...
@@ -484,6 +489,9 @@ static void init_disarm_motors()
...
@@ -484,6 +489,9 @@ static void init_disarm_motors()
motors
.
armed
(
false
);
motors
.
armed
(
false
);
// disable inertial nav errors temporarily
inertial_nav
.
ignore_next_error
();
compass
.
save_offsets
();
compass
.
save_offsets
();
g
.
throttle_cruise
.
save
();
g
.
throttle_cruise
.
save
();
...
...
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