diff --git a/ArduPlane/AP_Arming.h b/ArduPlane/AP_Arming.h index b00ff8b236..14c9ebed64 100644 --- a/ArduPlane/AP_Arming.h +++ b/ArduPlane/AP_Arming.h @@ -24,7 +24,14 @@ public: // var_info for holding Parameter information static const struct AP_Param::GroupInfo var_info[]; + bool disarm() override; + bool arm(AP_Arming::Method method, bool do_arming_checks=true) override; + + void update_soft_armed(); + protected: bool ins_checks(bool report) override; +private: + void change_arm_state(void); }; diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 67aff9c536..5fa7b7e98f 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -129,17 +129,17 @@ void Plane::loop() G_Dt = scheduler.get_loop_period_s(); } -void Plane::update_soft_armed() +void AP_Arming_Plane::update_soft_armed() { - hal.util->set_soft_armed(arming.is_armed() && + hal.util->set_soft_armed(is_armed() && hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED); - logger.set_vehicle_armed(hal.util->get_soft_armed()); + AP::logger().set_vehicle_armed(hal.util->get_soft_armed()); } // update AHRS system void Plane::ahrs_update() { - update_soft_armed(); + arming.update_soft_armed(); #if HIL_SUPPORT if (g.hil_mode == 1) { @@ -668,7 +668,7 @@ void Plane::disarm_if_autoland_complete() arming.is_armed()) { /* we have auto disarm enabled. See if enough time has passed */ if (millis() - auto_state.last_flying_ms >= landing.get_disarm_delay()*1000UL) { - if (disarm_motors()) { + if (arming.disarm()) { gcs().send_text(MAV_SEVERITY_INFO,"Auto disarmed"); } } diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 8d333a1057..153d1ede00 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -851,22 +851,6 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l plane.set_mode(plane.mode_auto, MODE_REASON_GCS_COMMAND); return MAV_RESULT_ACCEPTED; - case MAV_CMD_COMPONENT_ARM_DISARM: - if (is_equal(packet.param1,1.0f)) { - // run pre_arm_checks and arm_checks and display failures - const bool do_arming_checks = !is_equal(packet.param2,magic_force_arm_value); - if (plane.arm_motors(AP_Arming::Method::MAVLINK, do_arming_checks)) { - return MAV_RESULT_ACCEPTED; - } - return MAV_RESULT_FAILED; - } else if (is_zero(packet.param1)) { - if (plane.disarm_motors()) { - return MAV_RESULT_ACCEPTED; - } - return MAV_RESULT_FAILED; - } - return MAV_RESULT_UNSUPPORTED; - case MAV_CMD_DO_LAND_START: // attempt to switch to next DO_LAND_START command in the mission if (plane.mission.jump_to_landing_sequence()) { diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index 30e9bb86c9..afaebc1f01 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -210,24 +210,6 @@ void Plane::Log_Write_Sonar() logger.WriteBlock(&pkt, sizeof(pkt)); } -struct PACKED log_Arm_Disarm { - LOG_PACKET_HEADER; - uint64_t time_us; - uint8_t arm_state; - uint16_t arm_checks; -}; - -void Plane::Log_Arm_Disarm() { - struct log_Arm_Disarm pkt = { - LOG_PACKET_HEADER_INIT(LOG_ARM_DISARM_MSG), - time_us : AP_HAL::micros64(), - arm_state : arming.is_armed(), - arm_checks : arming.get_enabled_checks() - }; - logger.WriteCriticalBlock(&pkt, sizeof(pkt)); -} - - struct PACKED log_AETR { LOG_PACKET_HEADER; uint64_t time_us; @@ -276,8 +258,6 @@ const struct LogStructure Plane::log_structure[] = { "NTUN", "QfcccfffLLii", "TimeUS,Dist,TBrg,NavBrg,AltErr,XT,XTi,AspdE,TLat,TLng,TAlt,TAspd", "smddmmmnDUmn", "F0BBB0B0GGBB" }, { LOG_SONAR_MSG, sizeof(log_Sonar), "SONR", "QffBf", "TimeUS,Dist,Volt,Cnt,Corr", "smv--", "FB0--" }, - { LOG_ARM_DISARM_MSG, sizeof(log_Arm_Disarm), - "ARM", "QBH", "TimeUS,ArmState,ArmChecks", "s--", "F--" }, { LOG_ATRP_MSG, sizeof(AP_AutoTune::log_ATRP), "ATRP", "QBBcfff", "TimeUS,Type,State,Servo,Demanded,Achieved,P", "s---dd-", "F---00-" }, { LOG_STATUS_MSG, sizeof(log_Status), @@ -326,7 +306,6 @@ void Plane::Log_Write_Nav_Tuning() {} void Plane::Log_Write_Status() {} void Plane::Log_Write_Sonar() {} -void Plane::Log_Arm_Disarm() {} void Plane::Log_Write_RC(void) {} void Plane::Log_Write_Vehicle_Startup_Messages() {} diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 948cd64cf5..affdb13336 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -829,7 +829,6 @@ private: void Log_Write_Nav_Tuning(); void Log_Write_Status(); void Log_Write_Sonar(); - void Log_Arm_Disarm(); void Log_Write_RC(void); void Log_Write_Vehicle_Startup_Messages(); void Log_Write_AOA_SSA(); @@ -947,9 +946,6 @@ private: void startup_INS_ground(void); bool should_log(uint32_t mask); int8_t throttle_percentage(void); - void change_arm_state(void); - bool disarm_motors(void); - bool arm_motors(AP_Arming::Method method, bool do_arming_checks=true); bool auto_takeoff_check(void); void takeoff_calc_roll(void); void takeoff_calc_pitch(void); @@ -1052,7 +1048,6 @@ private: void publish_osd_info(); #endif void accel_cal_update(void); - void update_soft_armed(); #if SOARING_ENABLED == ENABLED void update_soaring(); #endif diff --git a/ArduPlane/RC_Channel.cpp b/ArduPlane/RC_Channel.cpp index bc9604102f..7bb7d1c563 100644 --- a/ArduPlane/RC_Channel.cpp +++ b/ArduPlane/RC_Channel.cpp @@ -58,20 +58,6 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::aux_func_t ch_option, void RC_Channel_Plane::do_aux_function(const aux_func_t ch_option, const aux_switch_pos_t ch_flag) { switch(ch_option) { - case AUX_FUNC::ARMDISARM: - // arm or disarm the vehicle - switch (ch_flag) { - case HIGH: - plane.arm_motors(AP_Arming::Method::AUXSWITCH, true); - break; - case MIDDLE: - // nothing - break; - case LOW: - plane.disarm_motors(); - break; - } - break; case AUX_FUNC::INVERTED: plane.inverted_flight = (ch_flag == HIGH); break; diff --git a/ArduPlane/afs_plane.cpp b/ArduPlane/afs_plane.cpp index 9d48b8dc82..0943166a1d 100644 --- a/ArduPlane/afs_plane.cpp +++ b/ArduPlane/afs_plane.cpp @@ -47,7 +47,7 @@ void AP_AdvancedFailsafe_Plane::terminate_vehicle(void) plane.quadplane.afs_terminate(); // also disarm to ensure that ignition is cut - plane.disarm_motors(); + plane.arming.disarm(); } void AP_AdvancedFailsafe_Plane::setup_IO_failsafe(void) diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 2305feec69..ca02ade46b 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -555,7 +555,7 @@ bool Plane::verify_takeoff() uint32_t now = AP_HAL::millis(); if (now - takeoff_state.start_time_ms > (uint32_t)(1000U * g2.takeoff_timeout)) { gcs().send_text(MAV_SEVERITY_INFO, "Takeoff timeout at %.1f m/s", ground_speed); - plane.disarm_motors(); + plane.arming.disarm(); mission.reset(); } } diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 6e2e748f24..b2ef0c8d96 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -117,7 +117,6 @@ enum log_messages { TYPE_GROUNDSTART_MSG, LOG_RC_MSG, LOG_SONAR_MSG, - LOG_ARM_DISARM_MSG, LOG_STATUS_MSG, LOG_QTUN_MSG, LOG_PARAMTUNE_MSG, @@ -145,7 +144,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) +// #define MASK_LOG_ARM_DISARM (1<<15) #define MASK_LOG_IMU_RAW (1UL<<19) // altitude control algorithms diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index 9c172284f0..fa22790874 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -174,7 +174,7 @@ void Plane::handle_battery_failsafe(const char *type_str, const int8_t action) snprintf(battery_type_str, 17, "%s battery", type_str); afs.gcs_terminate(true, battery_type_str); #else - disarm_motors(); + arming.disarm(); #endif break; diff --git a/ArduPlane/is_flying.cpp b/ArduPlane/is_flying.cpp index b93b22d4a2..d6b3168fe1 100644 --- a/ArduPlane/is_flying.cpp +++ b/ArduPlane/is_flying.cpp @@ -305,7 +305,7 @@ void Plane::crash_detection_update(void) } else { if (aparm.crash_detection_enable & CRASH_DETECT_ACTION_BITMASK_DISARM) { - disarm_motors(); + arming.disarm(); } if (crashed_near_land_waypoint) { gcs().send_text(MAV_SEVERITY_CRITICAL, "Hard landing detected"); diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 1a1a7e139e..e4546f0a0e 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2543,7 +2543,7 @@ void QuadPlane::check_land_complete(void) return; } if (land_detector(4000)) { - plane.disarm_motors(); + plane.arming.disarm(); poscontrol.state = QPOS_LAND_COMPLETE; gcs().send_text(MAV_SEVERITY_INFO,"Land complete"); // reload target airspeed which could have been modified by the mission diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index ded0136808..1d1e1d2899 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -146,7 +146,7 @@ void Plane::rudder_arm_disarm_check() } } else { //time to arm! - arm_motors(AP_Arming::Method::RUDDER); + arming.arm(AP_Arming::Method::RUDDER); rudder_arm_timer = 0; } } else { @@ -165,7 +165,7 @@ void Plane::rudder_arm_disarm_check() } } else { //time to disarm! - disarm_motors(); + arming.disarm(); rudder_arm_timer = 0; } } else { diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index f3cc5db356..7ba5b90d22 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -456,21 +456,18 @@ int8_t Plane::throttle_percentage(void) } /* - update AHRS soft arm state and log as needed - */ -void Plane::change_arm_state(void) + update HAL soft arm state and log as needed +*/ +void AP_Arming_Plane::change_arm_state(void) { - Log_Arm_Disarm(); + Log_Write_Arm_Disarm(); update_soft_armed(); - quadplane.set_armed(hal.util->get_soft_armed()); + plane.quadplane.set_armed(hal.util->get_soft_armed()); } -/* - arm motors - */ -bool Plane::arm_motors(const AP_Arming::Method method, const bool do_arming_checks) +bool AP_Arming_Plane::arm(const AP_Arming::Method method, const bool do_arming_checks) { - if (!arming.arm(method, do_arming_checks)) { + if (!AP_Arming::arm(method, do_arming_checks)) { return false; } @@ -481,18 +478,18 @@ bool Plane::arm_motors(const AP_Arming::Method method, const bool do_arming_chec /* disarm motors */ -bool Plane::disarm_motors(void) +bool AP_Arming_Plane::disarm(void) { - if (!arming.disarm()) { + if (!AP_Arming::disarm()) { return false; } - if (control_mode != &mode_auto) { + if (plane.control_mode != &plane.mode_auto) { // reset the mission on disarm if we are not in auto - mission.reset(); + plane.mission.reset(); } // suppress the throttle in auto-throttle modes - throttle_suppressed = auto_throttle_mode; + plane.throttle_suppressed = plane.auto_throttle_mode; //only log if disarming was successful change_arm_state(); @@ -502,10 +499,10 @@ bool Plane::disarm_motors(void) #if QAUTOTUNE_ENABLED //save qautotune gains if enabled and success - if (control_mode == &mode_qautotune) { - quadplane.qautotune.save_tuning_gains(); + if (plane.control_mode == &plane.mode_qautotune) { + plane.quadplane.qautotune.save_tuning_gains(); } else { - quadplane.qautotune.reset(); + plane.quadplane.qautotune.reset(); } #endif