diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp
new file mode 100644
index 0000000000..b0519bd3e3
--- /dev/null
+++ b/libraries/AP_Arming/AP_Arming.cpp
@@ -0,0 +1,264 @@
+/// -*- 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 .
+ */
+
+#include
+#include
+
+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 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;
+}
+
+uint8_t AP_Arming::arming_required() {
+ return require.get();
+}
+
+bool AP_Arming::rudder_arming_enabled() {
+ if (disable_rudder_arm == 0)
+ return true;
+
+ return false;
+}
+
+uint8_t AP_Arming::get_arming_method() {
+ return arming_method;
+}
diff --git a/libraries/AP_Arming/AP_Arming.h b/libraries/AP_Arming/AP_Arming.h
new file mode 100644
index 0000000000..0b65138aac
--- /dev/null
+++ b/libraries/AP_Arming/AP_Arming.h
@@ -0,0 +1,97 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#ifndef __AP_ARMING_H__
+#define __AP_ARMING_H__
+
+#include
+#include
+#include
+#include
+
+class AP_Arming {
+public:
+ enum ArmingChecks {
+ ARMING_CHECK_NONE = 0x0000,
+ ARMING_CHECK_ALL = 0x0001,
+ ARMING_CHECK_BARO = 0x0002,
+ ARMING_CHECK_COMPASS = 0x0004,
+ ARMING_CHECK_GPS = 0x0008,
+ ARMING_CHECK_INS = 0x0010,
+ ARMING_CHECK_PARAMETERS = 0x0020,
+ ARMING_CHECK_RC = 0x0040,
+ ARMING_CHECK_VOLTAGE = 0x0080,
+ ARMING_CHECK_BATTERY = 0x0100,
+ };
+
+ enum ArmingMethod {
+ NONE = 0,
+ RUDDER,
+ MAVLINK
+ };
+
+ enum ArmingRequired {
+ NO = 0,
+ YES_MIN_PWM,
+ YES_ZERO_PWM,
+ };
+
+ //for the hacky funciton pointer to gcs_send_text_p
+ typedef void (*gcs_send_t_p)(gcs_severity, const prog_char_t*);
+
+ //for params
+ static const struct AP_Param::GroupInfo var_info[];
+
+protected:
+ bool armed;
+
+ //Parameters
+ AP_Int8 require;
+ AP_Int8 disable_rudder_arm;
+ //bitmask for which checks are required
+ AP_Int16 checks_to_perform;
+
+ //how the vehicle was armed
+ uint8_t arming_method;
+
+ const AP_AHRS &ahrs;
+ const AP_Baro &barometer;
+ const bool &home_is_set;
+ gcs_send_t_p gcs_send_text_P;
+
+public:
+ AP_Arming(const AP_AHRS &ahrs_ref, const AP_Baro &baro,
+ const bool &home_set, gcs_send_t_p);
+
+ bool is_armed();
+
+ uint16_t get_enabled_checks();
+
+ void set_enabled_checks(uint16_t);
+
+ bool barometer_checks();
+
+ bool compass_checks();
+
+ bool gps_checks();
+
+ bool battery_checks();
+
+ bool hardware_safety_check();
+
+ bool manual_transmitter_checks();
+
+ bool pre_arm_checks();
+
+ bool arm(uint8_t method);
+
+ bool disarm();
+
+ uint8_t arming_required();
+
+ bool rudder_arming_enabled();
+
+ uint8_t get_arming_method();
+
+};
+
+#endif //__AP_ARMING_H__