From b0a0316dd69fe8a9ecabd54516f74126aa10188f Mon Sep 17 00:00:00 2001 From: Michael Day Date: Wed, 27 Nov 2013 18:19:34 -0800 Subject: [PATCH] Plane: Now using AP_Arming library. --- ArduPlane/ArduPlane.pde | 16 +++++++++-- ArduPlane/Attitude.pde | 18 ++++++++++++ ArduPlane/GCS_Mavlink.pde | 23 ++++++++++++++++ ArduPlane/Log.pde | 18 ++++++++++++ ArduPlane/Parameters.h | 3 ++ ArduPlane/Parameters.pde | 4 +++ ArduPlane/defines.h | 2 ++ ArduPlane/radio.pde | 58 +++++++++++++++++++++++++++++++++++++++ 8 files changed, 139 insertions(+), 3 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index c90613d004..c518d8c9e0 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -73,6 +73,8 @@ #include // Notify library #include // Battery monitor library +#include + // Pre-AP_HAL compatibility #include "compat.h" @@ -697,6 +699,11 @@ AP_Mount camera_mount(¤t_loc, g_gps, ahrs, 0); AP_Mount camera_mount2(¤t_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])); } diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index df0ae0fc7e..a99a24f60e 100644 --- a/ArduPlane/Attitude.pde +++ b/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(); diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 92154922eb..3eb1b12d27 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/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: diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde index c2e055605b..670a055f39 100644 --- a/ArduPlane/Log.pde +++ b/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), }; diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 7fdfdd6f67..d3204040bf 100644 --- a/ArduPlane/Parameters.h +++ b/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 diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index 9654d99d83..de7c612b5b 100644 --- a/ArduPlane/Parameters.pde +++ b/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), diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 8a3f1be658..588403bc9e 100644 --- a/ArduPlane/defines.h +++ b/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 // ---------------- diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index 9b6bed5e4e..76d658e128 100644 --- a/ArduPlane/radio.pde +++ b/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)