AP_Arming: First commit of AP_Arming library.

Intended to prevent unintentional motor startups.

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 <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 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;
+}

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 <AP_AHRS.h>
+#include <AP_HAL.h>
+#include <AP_Param.h>
+#include <GCS_MAVLink.h>
+
+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__