Plane: Now using AP_Arming library.

ArduPlane/ArduPlane.pde

@@ -73,6 +73,8 @@
 #include <AP_Notify.h>      // Notify library
 #include <AP_BattMonitor.h> // Battery monitor library
 
+#include <AP_Arming.h>
+
 // Pre-AP_HAL compatibility
 #include "compat.h"
 
@@ -697,6 +699,11 @@ AP_Mount camera_mount(&current_loc, g_gps, ahrs, 0);
 AP_Mount camera_mount2(&current_loc, g_gps, ahrs, 1);
 #endif
 
+////////////////////////////////////////////////////////////////////////////////
+// Arming/Disarming mangement class
+////////////////////////////////////////////////////////////////////////////////
+AP_Arming arming(ahrs, barometer, home_is_set, gcs_send_text_P);
+
 ////////////////////////////////////////////////////////////////////////////////
 // Top-level logic
 ////////////////////////////////////////////////////////////////////////////////
@@ -756,9 +763,6 @@ void setup() {
     // load the default values of variables listed in var_info[]
     AP_Param::setup_sketch_defaults();
 
-    // arduplane does not use arming nor pre-arm checks
-    AP_Notify::flags.armed = true;
-    AP_Notify::flags.pre_arm_check = true;
     AP_Notify::flags.failsafe_battery = false;
 
     notify.init(false);
@@ -771,6 +775,12 @@ void setup() {
 
     init_ardupilot();
 
+    //if no user intervention require to arm, then just arm already
+    //(maintain old behavior)
+    if (arming.arming_required() == AP_Arming::NO) {
+        arming.arm(AP_Arming::NONE);
+    }
+
     // initialise the main loop scheduler
     scheduler.init(&scheduler_tasks[0], sizeof(scheduler_tasks)/sizeof(scheduler_tasks[0]));
 }

ArduPlane/Attitude.pde

@@ -871,6 +871,24 @@ static void set_servos(void)
         channel_output_mixer(g.elevon_output, channel_pitch->radio_out, channel_roll->radio_out);
     }
 
+    //send throttle to 0 or MIN_PWM if not yet armed
+    if (!arming.is_armed()) {
+        //Some ESCs get noisy (beep error msgs) if PWM == 0.
+        //This little segment aims to avoid this.
+        switch (arming.arming_required()) { 
+            case AP_Arming::YES_MIN_PWM:
+                channel_throttle->radio_out = channel_throttle->radio_min;
+            break;
+            case AP_Arming::YES_ZERO_PWM:
+                channel_throttle->radio_out = 0;
+            break;
+            default:
+                //keep existing behavior: do nothing to radio_out
+                //(don't disarm throttle channel even if AP_Arming class is)
+            break;
+        }
+    }
+
     // send values to the PWM timers for output
     // ----------------------------------------
     channel_roll->output();

ArduPlane/GCS_Mavlink.pde

@@ -1280,6 +1280,29 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             result = MAV_RESULT_ACCEPTED;
             break;
 
+        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 arm_checks and display failures
+                    if (arming.arm(AP_Arming::MAVLINK)) {
+                        //only log if arming was successful
+                        Log_Arm_Disarm();
+                    }
+                    result = MAV_RESULT_ACCEPTED;
+                } else if (packet.param1 == 0.0f)  {
+                    if (arming.disarm()) {
+                        //only log if disarming was successful
+                        Log_Arm_Disarm();
+                    }
+                    result = MAV_RESULT_ACCEPTED;
+                } else {
+                    result = MAV_RESULT_UNSUPPORTED;
+                }
+            } else {
+                result = MAV_RESULT_UNSUPPORTED;
+            }
+            break;
+
         case MAV_CMD_DO_SET_MODE:
             switch ((uint16_t)packet.param1) {
             case MAV_MODE_MANUAL_ARMED:

ArduPlane/Log.pde

@@ -426,6 +426,13 @@ struct PACKED log_Current {
     float   current_total;
 };
 
+struct PACKED log_Arm_Disarm {
+    LOG_PACKET_HEADER;
+    uint32_t time_ms;
+    uint8_t  arm_state;
+    uint16_t arm_checks;
+};
+
 static void Log_Write_Current()
 {
     struct log_Current pkt = {
@@ -440,6 +447,15 @@ static void Log_Write_Current()
     DataFlash.WriteBlock(&pkt, sizeof(pkt));
 }
 
+static void Log_Arm_Disarm() {
+    struct log_Arm_Disarm pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_ARM_DISARM_MSG),
+        time_ms                 : hal.scheduler->millis(),
+        arm_state               : arming.is_armed(),
+        arm_checks              : arming.get_enabled_checks()      
+    };
+    DataFlash.WriteBlock(&pkt, sizeof(pkt));
+}
 
 struct PACKED log_Compass {
     LOG_PACKET_HEADER;
@@ -529,6 +545,8 @@ static const struct LogStructure log_structure[] PROGMEM = {
       "MAG", "Ihhhhhh",   "TimeMS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ" },
     { LOG_COMPASS2_MSG, sizeof(log_Compass),             
       "MAG2", "Ihhhhhh",   "TimeMS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ" },
+    { LOG_ARM_DISARM_MSG, sizeof(log_Arm_Disarm),
+      "ARM", "IHB", "TimeMS,ArmState,ArmChecks" },
     TECS_LOG_FORMAT(LOG_TECS_MSG),
 };
 

ArduPlane/Parameters.h

@@ -100,6 +100,9 @@ public:
         k_param_hil_err_limit,
         k_param_sonar,
 
+        // 100: Arming parameters
+        k_param_arming = 100,
+
         // 110: Telemetry control
         //
         k_param_gcs0 = 110,         // stream rates for uartA

ArduPlane/Parameters.pde

@@ -772,6 +772,10 @@ const AP_Param::Info var_info[] PROGMEM = {
     GOBJECT(camera,                  "CAM_", AP_Camera),
 #endif
 
+    // @Group: ARMING_
+    // @Path: ../libararies/AP_Arming/AP_Arming.cpp
+    GOBJECT(arming,                 "ARMING_", AP_Arming),
+
     // @Group: RELAY_
     // @Path: ../libraries/AP_Relay/AP_Relay.cpp
     GOBJECT(relay,                  "RELAY_", AP_Relay),

ArduPlane/defines.h

@@ -165,6 +165,7 @@ enum log_messages {
     LOG_RC_MSG,
     LOG_SONAR_MSG,
     LOG_COMPASS2_MSG,
+    LOG_ARM_DISARM_MSG,
     MAX_NUM_LOGS // always at the end
 };
 
@@ -183,6 +184,7 @@ enum log_messages {
 #define MASK_LOG_CAMERA                 (1<<12)
 #define MASK_LOG_RC                     (1<<13)
 #define MASK_LOG_SONAR                  (1<<14)
+#define MASK_LOG_ARM_DISARM             (1<<15)
 
 // Waypoint Modes
 // ----------------

ArduPlane/radio.pde

@@ -62,6 +62,62 @@ static void init_rc_out()
 #endif
 }
 
+// check for pilot input on rudder stick for arming
+static void rudder_arm_check() {
+    //TODO: ensure rudder arming disallowed during radio calibration
+
+    //TODO: waggle ailerons and rudder and beep after rudder arming
+    
+    static uint32_t rudder_arm_timer = 0;
+
+    if (arming.is_armed()) {
+        //already armed, no need to run remainder of this function
+        rudder_arm_timer = 0;
+        return;
+    } 
+
+    if (! arming.rudder_arming_enabled()) {
+        //parameter disallows rudder arming
+        return;
+    }
+
+    //if throttle is not down, then pilot cannot rudder arm
+    if(g.rc_3.control_in > 0 ) {
+        rudder_arm_timer = 0;
+        return;
+    }
+
+    //if not in a 'manual' mode then disallow rudder arming
+    if (auto_throttle_mode ) {
+        rudder_arm_timer = 0;
+        return;      
+    }
+
+    // full left or right rudder starts arming counter
+    if (g.rc_4.control_in > 4000 
+            || g.rc_4.control_in < -4000) {
+        
+        uint32_t now = millis();
+
+        if (rudder_arm_timer == 0 || 
+              now - rudder_arm_timer < 3000) {
+
+            if (rudder_arm_timer == 0) rudder_arm_timer = now;
+        } else {
+            //time to arm!
+            if (arming.arm(AP_Arming::RUDDER)) {
+                //only log if arming was successful
+                Log_Arm_Disarm();
+            }                
+        }
+
+    }
+    else { //not at full left or right rudder
+        rudder_arm_timer = 0;
+        return;
+    }
+}
+
 static void read_radio()
 {
     elevon.ch1_temp = channel_roll->read();
@@ -110,6 +166,8 @@ static void read_radio()
         airspeed_nudge_cm = 0;
         throttle_nudge = 0;
     }
+
+    rudder_arm_check();
 }
 
 static void control_failsafe(uint16_t pwm)