Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 01:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: pre-arm check speed is less than 50cm/s

Browse Source
This commit is contained in:
Randy Mackay 2013-10-20 21:56:00 +09:00
parent 943d7374f6
commit 6b9b5c5617
2 changed files with 22 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduCopter/config.h
View File

@ -393,6 +393,11 @@
#define FS_GCS_ENABLED_ALWAYS_RTL 1 #define FS_GCS_ENABLED_ALWAYS_RTL 1
#define FS_GCS_ENABLED_CONTINUE_MISSION 2 #define FS_GCS_ENABLED_CONTINUE_MISSION 2
// pre-arm check max velocity
#ifndef PREARM_MAX_VELOCITY_CMS
# define PREARM_MAX_VELOCITY_CMS 50.0f // vehicle must be travelling under 50cm/s before arming
#endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// MAGNETOMETER // MAGNETOMETER
#ifndef MAGNETOMETER #ifndef MAGNETOMETER

20
ArduCopter/motors.pde
View File

@ -292,7 +292,7 @@ static void pre_arm_checks(bool display_failure)
#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) && g_gps->hdop > g.gps_hdop_good)) { if(!fence.pre_arm_check() || (((fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) != 0) && !pre_arm_gps_checks())) {
if (display_failure) { if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Bad GPS Pos")); gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Bad GPS Pos"));
} }
@ -330,7 +330,7 @@ static void pre_arm_checks(bool display_failure)
} }
// 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) && (!GPS_ok() || g_gps->hdop > g.gps_hdop_good || gps_glitch.glitching())) { if (mode_requires_GPS(control_mode) && !pre_arm_gps_checks()) {
if (display_failure) { if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Bad GPS Pos")); gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Bad GPS Pos"));
} }
@ -368,6 +368,20 @@ static void pre_arm_rc_checks()
ap.pre_arm_rc_check = true; ap.pre_arm_rc_check = true;
} }
// performs pre_arm gps related checks and returns true if passed
static bool pre_arm_gps_checks()
{
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
if (!GPS_ok() || g_gps->hdop > g.gps_hdop_good || gps_glitch.glitching() || speed_cms == 0 || speed_cms > PREARM_MAX_VELOCITY_CMS) {
return false;
}
// if we got here all must be ok
return true;
}
// arm_checks - perform final checks before arming // arm_checks - perform final checks before arming
// always called just before arming. Return true if ok to arm // always called just before arming. Return true if ok to arm
static bool arm_checks(bool display_failure) static bool arm_checks(bool display_failure)
@ -378,7 +392,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(mode_requires_GPS(control_mode) && (!GPS_ok() || g_gps->hdop > g.gps_hdop_good || gps_glitch.glitching())) { if (mode_requires_GPS(control_mode) && !pre_arm_gps_checks()) {
if (display_failure) { if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Bad GPS Pos")); gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Bad GPS Pos"));
} }