You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
275 lines
8.3 KiB
275 lines
8.3 KiB
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- |
|
/* |
|
This program is free software: you can redistribute it and/or modify |
|
it under the terms of the GNU General Public License as published by |
|
the Free Software Foundation, either version 3 of the License, or |
|
(at your option) any later version. |
|
|
|
This program is distributed in the hope that it will be useful, |
|
but WITHOUT ANY WARRANTY; without even the implied warranty of |
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
|
GNU General Public License for more details. |
|
|
|
You should have received a copy of the GNU General Public License |
|
along with this program. If not, see <http://www.gnu.org/licenses/>. |
|
*/ |
|
|
|
#include <AP_Arming.h> |
|
#include <AP_Notify.h> |
|
|
|
extern const AP_HAL::HAL& hal; |
|
|
|
const AP_Param::GroupInfo AP_Arming::var_info[] PROGMEM = { |
|
// @Param: REQUIRE |
|
// @DisplayName: Require Arming Motors |
|
// @Description: Arming disabled until some requirements are met. If 0, there are no requirements (arm immediately). If 1, require rudder stick or GCS arming before arming motors and send THR_MIN PWM to throttle channel when disarmed. If 2, require rudder stick or GCS arming and send 0 PWM to throttle channel when disarmed. See the ARMING_CHECK_* parameters to see what checks are done before arming. Note, if setting this parameter to 0 a reboot is required to arm the plane. Also note, even with this parameter at 0, if ARMING_CHECK parameter is not also zero the plane may fail to arm throttle at boot due to a pre-arm check failure. |
|
// @Values: 0:Disabled,1:THR_MIN PWM when disarmed,2:0 PWM when disarmed |
|
// @User: Advanced |
|
AP_GROUPINFO("REQUIRE", 0, AP_Arming, require, 0), |
|
|
|
// @Param: DIS_RUD |
|
// @DisplayName: Disable Rudder Arming |
|
// @Description: Do not allow arming via the rudder input stick. |
|
// @Values: 0:Disabled (Rudder Arming Allowed),1:Enabled(No Rudder Arming) |
|
// @User: Advanced |
|
AP_GROUPINFO("DIS_RUD", 1, AP_Arming, disable_rudder_arm, 0), |
|
|
|
// @Param: CHECK |
|
// @DisplayName: Arm Checks to Peform (bitmask) |
|
// @Description: Checks prior to arming motor. |
|
// @Values: 0:None,1:All,2:Barometer,4:Compass,8:GPS,16:INS,32:Parameters,64:Manual RC Trasmitter,128:Board voltage,256:Battery Level |
|
// @User: Advanced |
|
AP_GROUPINFO("CHECK", 2, AP_Arming, checks_to_perform, 0), |
|
|
|
AP_GROUPEND |
|
}; |
|
|
|
//The function point is particularly hacky, hacky, tacky |
|
//but I don't want to reimplement messaging to GCS at the moment: |
|
AP_Arming::AP_Arming(const AP_AHRS &ahrs_ref, const AP_Baro &baro, |
|
const bool &home_set, gcs_send_t_p gcs_print_func) |
|
: armed(false) |
|
, arming_method(NONE) |
|
, ahrs(ahrs_ref) |
|
, barometer(baro) |
|
, home_is_set(home_set) |
|
, gcs_send_text_P(gcs_print_func) |
|
{ |
|
AP_Param::setup_object_defaults(this, var_info); |
|
} |
|
|
|
bool AP_Arming::is_armed() |
|
{ |
|
return require == NONE || armed; |
|
} |
|
|
|
uint16_t AP_Arming::get_enabled_checks() |
|
{ |
|
return checks_to_perform; |
|
} |
|
|
|
void AP_Arming::set_enabled_checks(uint16_t ap) { |
|
checks_to_perform = ap; |
|
} |
|
|
|
bool AP_Arming::barometer_checks() |
|
{ |
|
if ((checks_to_perform & ARMING_CHECK_ALL) || |
|
(checks_to_perform & ARMING_CHECK_BARO)) { |
|
if (! barometer.healthy) { |
|
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Baro not healthy!")); |
|
return false; |
|
} |
|
} |
|
|
|
return true; |
|
} |
|
|
|
bool AP_Arming::compass_checks() |
|
{ |
|
if ((checks_to_perform) & ARMING_CHECK_ALL || |
|
(checks_to_perform) & ARMING_CHECK_COMPASS) { |
|
const Compass* compass = ahrs.get_compass(); |
|
|
|
//if there is no compass and the user has specifically asked to check |
|
//the compass, then there is a problem |
|
if (compass == NULL && (checks_to_perform & ARMING_CHECK_COMPASS)) { |
|
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: No compass detected.")); |
|
return false; |
|
} else if (compass == NULL) { |
|
//if the user's not asking to check and there isn't a compass |
|
//then skip compass checks |
|
return true; |
|
} |
|
|
|
if (! compass->healthy()) { |
|
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Compass not healthy!")); |
|
return false; |
|
} |
|
// check compass learning is on or offsets have been set |
|
Vector3f offsets = compass->get_offsets(); |
|
if(!compass->_learn && offsets.length() == 0) { |
|
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Compass not calibrated")); |
|
return false; |
|
} |
|
|
|
} |
|
|
|
return true; |
|
} |
|
|
|
bool AP_Arming::gps_checks() |
|
{ |
|
if ((checks_to_perform & ARMING_CHECK_ALL) || |
|
(checks_to_perform & ARMING_CHECK_GPS)) { |
|
const GPS *gps = ahrs.get_gps(); |
|
|
|
//If no GPS and the user has specifically asked to check GPS, then |
|
//there is a problem |
|
if (gps == NULL && (checks_to_perform & ARMING_CHECK_GPS)) { |
|
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: No GPS detected.")); |
|
return false; |
|
} else if (gps == NULL) { |
|
//assume the user doesn't have a GPS on purpose |
|
return true; |
|
} |
|
|
|
//GPS OK? |
|
if (!home_is_set || gps->status() != GPS::GPS_OK_FIX_3D || |
|
AP_Notify::flags.gps_glitching || |
|
AP_Notify::flags.failsafe_gps) { |
|
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Bad GPS Pos")); |
|
return false; |
|
} |
|
} |
|
|
|
return true; |
|
} |
|
|
|
bool AP_Arming::battery_checks() |
|
{ |
|
if ((checks_to_perform & ARMING_CHECK_ALL) || |
|
(checks_to_perform & ARMING_CHECK_BATTERY)) { |
|
|
|
if (AP_Notify::flags.failsafe_battery) { |
|
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Battery failsafe on.")); |
|
return false; |
|
} |
|
} |
|
|
|
return true; |
|
} |
|
|
|
bool AP_Arming::hardware_safety_check() |
|
{ |
|
// check if safety switch has been pushed |
|
if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) { |
|
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Hardware Safety Switch")); |
|
return false; |
|
} |
|
|
|
return true; |
|
} |
|
|
|
bool AP_Arming::manual_transmitter_checks() |
|
{ |
|
if ((checks_to_perform & ARMING_CHECK_ALL) || |
|
(checks_to_perform & ARMING_CHECK_RC)) { |
|
|
|
if (AP_Notify::flags.failsafe_radio) { |
|
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Radio failsafe on.")); |
|
return false; |
|
} |
|
|
|
//TODO verify radio calibration |
|
//Requires access to Parameters ... which are implemented a little |
|
//differently for Rover, Plane, and Copter. |
|
} |
|
|
|
return true; |
|
} |
|
|
|
bool AP_Arming::pre_arm_checks() |
|
{ |
|
if (! hardware_safety_check()) |
|
return false; |
|
|
|
if (! barometer_checks()) |
|
return false; |
|
|
|
if (! compass_checks()) |
|
return false; |
|
|
|
if (! gps_checks()) |
|
return false; |
|
|
|
if (! battery_checks()) |
|
return false; |
|
|
|
if (! manual_transmitter_checks()) |
|
return false; |
|
|
|
//all checks passed, allow arming! |
|
return true; |
|
} |
|
|
|
//returns true if arming occured successfully |
|
bool AP_Arming::arm(uint8_t method) |
|
{ |
|
if (armed) { //already armed |
|
return false; |
|
} |
|
|
|
//are arming checks disabled? |
|
if (checks_to_perform == ARMING_CHECK_NONE) { |
|
armed = true; |
|
arming_method = NONE; |
|
gcs_send_text_P(SEVERITY_HIGH,PSTR("Throttle armed!")); |
|
return true; |
|
} |
|
|
|
if (pre_arm_checks()) { |
|
armed = true; |
|
arming_method = method; |
|
|
|
gcs_send_text_P(SEVERITY_HIGH,PSTR("Throttle armed!")); |
|
|
|
//TODO: Log motor arming to the dataflash |
|
//Can't do this from this class until there is a unified logging library |
|
|
|
} else { |
|
armed = false; |
|
arming_method = NONE; |
|
} |
|
|
|
return armed; |
|
} |
|
|
|
//returns true if disarming occurred successfully |
|
bool AP_Arming::disarm() |
|
{ |
|
if (! armed) { // already disarmed |
|
return false; |
|
} |
|
armed = false; |
|
|
|
gcs_send_text_P(SEVERITY_HIGH,PSTR("Throttle disarmed!")); |
|
|
|
//TODO: Log motor disarming to the dataflash |
|
//Can't do this from this class until there is a unified logging library. |
|
|
|
return true; |
|
} |
|
|
|
AP_Arming::ArmingRequired AP_Arming::arming_required() |
|
{ |
|
return (AP_Arming::ArmingRequired)require.get(); |
|
} |
|
|
|
bool AP_Arming::rudder_arming_enabled() |
|
{ |
|
if (disable_rudder_arm == 0) |
|
return true; |
|
|
|
return false; |
|
}
|
|
|