aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: add arm check of GPS hdop when in Loiter

Browse Source
This commit is contained in:
Randy Mackay 2013-09-09 14:03:40 +09:00
parent 1abb439051
commit 1d55fa6818
2 changed files with 29 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/GCS_Mavlink.pde
View File

@ -1218,9 +1218,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAV_CMD_COMPONENT_ARM_DISARM:
if (packet.target_component == MAV_COMP_ID_SYSTEM_CONTROL) {
if (packet.param1 == 1.0f) {
// run pre-arm-checks and display failures
// run pre_arm_checks and arm_checks and display failures
pre_arm_checks(true);
if(ap.pre_arm_check) {
if(ap.pre_arm_check && arm_checks(true)) {
init_arm_motors();
}
result = MAV_RESULT_ACCEPTED;

28
ArduCopter/motors.pde
View File

@ -59,7 +59,7 @@ static void arm_motors_check()
if (arming_counter == ARM_DELAY && !motors.armed()) {
// run pre-arm-checks and display failures
pre_arm_checks(true);
if(ap.pre_arm_check) {
if(ap.pre_arm_check && arm_checks(true)) {
init_arm_motors();
}else{
// reset arming counter if pre-arm checks fail
@ -327,6 +327,11 @@ static void pre_arm_checks(bool display_failure)
return;
}
// pass arming checks at least once
if (!arm_checks(display_failure)) {
return;
}
// if we've gotten this far then pre arm checks have completed
set_pre_arm_check(true);
}
@ -358,6 +363,27 @@ static void pre_arm_rc_checks()
ap.pre_arm_rc_check = true;
}
// arm_checks - perform final checks before arming
// always called just before arming. Return true if ok to arm
static bool arm_checks(bool display_failure)
{
// succeed if arming checks are disabled
if(!g.arming_check_enabled) {
return true;
}
// check gps is ok if required
if(mode_requires_GPS(control_mode) && (!GPS_ok() || g_gps->hdop > g.gps_hdop_good)) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Bad GPS Pos"));
}
return false;
}
// if we've gotten this far all is ok
return true;
}
static void init_disarm_motors()
{
#if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL