Line data    Source code

       1             : #include "AP_Arming_Sub.h"
       2             : #include "Sub.h"
       3             : 
       4        4767 : bool AP_Arming_Sub::rc_calibration_checks(bool display_failure)
       5             : {
       6             :     const RC_Channel *channels[] = {
       7        4767 :         sub.channel_roll,
       8        4767 :         sub.channel_pitch,
       9        4767 :         sub.channel_throttle,
      10        4767 :         sub.channel_yaw
      11        4767 :     };
      12        4767 :     return rc_checks_copter_sub(display_failure, channels);
      13             : }
      14             : 
      15        4688 : bool AP_Arming_Sub::has_disarm_function() const {
      16        9376 :     if (sub.jsbutton_function_is_assigned(JSButton::k_arm_toggle) ||
      17        4688 :         sub.jsbutton_function_is_assigned(JSButton::k_disarm)) {
      18        4688 :         return true;
      19             :     }
      20             :     // check if an AUX function that disarms or estops is setup
      21           0 :     if (rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_ESTOP) || 
      22           0 :         rc().find_channel_for_option(RC_Channel::AUX_FUNC::DISARM) || 
      23           0 :         rc().find_channel_for_option(RC_Channel::AUX_FUNC::ARMDISARM) || 
      24           0 :         rc().find_channel_for_option(RC_Channel::AUX_FUNC::ARM_EMERGENCY_STOP)) {
      25           0 :         return true;
      26             :     }  
      27           0 :     return false;
      28             : }
      29             : 
      30       10867 : bool AP_Arming_Sub::pre_arm_checks(bool display_failure)
      31             : {
      32       10867 :     if (armed) {
      33        6179 :         return true;
      34             :     }
      35             : 
      36        4688 :     if (!hal.scheduler->is_system_initialized()) {
      37           0 :         check_failed(display_failure, "System not initialised");
      38           0 :         return false;
      39             :     }
      40             : 
      41             :     // don't allow arming unless there is a disarm button configured
      42        4688 :     if (!has_disarm_function()) {
      43           0 :         check_failed(display_failure, "Must assign a disarm or arm_toggle button or disarm aux function");
      44           0 :         return false;
      45             :     }
      46             : 
      47        4688 :     return AP_Arming::pre_arm_checks(display_failure);
      48             : }
      49             : 
      50        4688 : bool AP_Arming_Sub::ins_checks(bool display_failure)
      51             : {
      52             :     // call parent class checks
      53        4688 :     if (!AP_Arming::ins_checks(display_failure)) {
      54           0 :         return false;
      55             :     }
      56             : 
      57             :     // additional sub-specific checks
      58        4688 :     if (check_enabled(Check::INS)) {
      59           0 :         char failure_msg[50] = {};
      60           0 :         if (!AP::ahrs().pre_arm_check(false, failure_msg, sizeof(failure_msg))) {
      61           0 :             check_failed(Check::INS, display_failure, "AHRS: %s", failure_msg);
      62           0 :             return false;
      63             :         }
      64             :     }
      65             : 
      66        4688 :     return true;
      67             : }
      68             : 
      69          47 : bool AP_Arming_Sub::arm(AP_Arming::Method method, bool do_arming_checks)
      70             : {
      71             :     static bool in_arm_motors = false;
      72             : 
      73             :     // exit immediately if already in this function
      74          47 :     if (in_arm_motors) {
      75           0 :         return false;
      76             :     }
      77             : 
      78          47 :     in_arm_motors = true;
      79             : 
      80             :     //if RC checks enabled, and RC_OPTIONS enabled for "0" throttle, and enabled check for throttle within trim position
      81          94 :     if (check_enabled(Check::RC) &&
      82          94 :      rc().option_is_enabled(RC_Channels::Option::ARMING_CHECK_THROTTLE) &&
      83           0 :      (sub.g.thr_arming_position == WITHIN_THR_TRIM)) {
      84           0 :         const char *rc_item = "Throttle";
      85             :         // check throttle is within trim+/- dz, ie centered throttle
      86           0 :         if (!sub.channel_throttle->in_trim_dz()) {
      87           0 :            check_failed(Check::RC, true, "%s not centered/close to trim", rc_item);
      88           0 :            AP_Notify::events.arming_failed = true;
      89           0 :            in_arm_motors = false;
      90           0 :            return false;
      91             :         }
      92             :     }
      93             : 
      94          47 :     if (!AP_Arming::arm(method, do_arming_checks)) {
      95           0 :         AP_Notify::events.arming_failed = true;
      96           0 :         in_arm_motors = false;
      97           0 :         return false;
      98             :     }
      99             : 
     100             : #if HAL_LOGGING_ENABLED
     101             :     // let logger know that we're armed (it may open logs e.g.)
     102          47 :     AP::logger().set_vehicle_armed(true);
     103             : #endif
     104             : 
     105             :     // disable cpu failsafe because initialising everything takes a while
     106          47 :     sub.mainloop_failsafe_disable();
     107             : 
     108             :     // notify that arming will occur (we do this early to give plenty of warning)
     109          47 :     AP_Notify::flags.armed = true;
     110             :     // call notify update a few times to ensure the message gets out
     111         564 :     for (uint8_t i=0; i<=10; i++) {
     112         517 :         AP::notify().update();
     113             :     }
     114             : 
     115          47 :     send_arm_disarm_statustext("Arming motors");
     116             : 
     117          47 :     AP_AHRS &ahrs = AP::ahrs();
     118             : 
     119          47 :     sub.initial_armed_bearing = ahrs.yaw_sensor;
     120             : 
     121          47 :     if (!ahrs.home_is_set()) {
     122             :         // Reset EKF altitude if home hasn't been set yet (we use EKF altitude as substitute for alt above home)
     123             : 
     124             :         // Always use absolute altitude for ROV
     125             :         // ahrs.resetHeightDatum();
     126             :         // AP::logger().Write_Event(LogEvent::EKF_ALT_RESET);
     127          46 :     } else if (!ahrs.home_is_locked()) {
     128             :         // Reset home position if it has already been set before (but not locked)
     129          46 :         if (!sub.set_home_to_current_location(false)) {
     130             :             // ignore this failure
     131             :         }
     132             :     }
     133             : 
     134          47 :     hal.util->set_soft_armed(true);
     135             : 
     136             :     // enable output to motors
     137          47 :     sub.enable_motor_output();
     138             : 
     139             :     // finally actually arm the motors
     140          47 :     sub.motors.armed(true);
     141             : 
     142             : #if HAL_LOGGING_ENABLED
     143             :     // log flight mode in case it was changed while vehicle was disarmed
     144          47 :     AP::logger().Write_Mode((uint8_t)sub.control_mode, sub.control_mode_reason);
     145             : #endif
     146             : 
     147             :     // reenable failsafe
     148          47 :     sub.mainloop_failsafe_enable();
     149             : 
     150             :     // perf monitor ignores delay due to arming
     151          47 :     AP::scheduler().perf_info.ignore_this_loop();
     152             : 
     153             :     // flag exiting this function
     154          47 :     in_arm_motors = false;
     155             : 
     156             :     // if we do not have an ekf origin then we can't use the WMM tables
     157          47 :     Location origin_loc;
     158          47 :     if (!ahrs.get_origin(origin_loc)) {
     159           1 :         gcs().send_text(MAV_SEVERITY_WARNING, "Compass performance degraded");
     160           1 :         if (check_enabled(Check::PARAMETERS)) {
     161           0 :             check_failed(Check::PARAMETERS, true, "No world position, check AHRS_ORIGIN_* parameters");
     162           0 :             return false;
     163             :         }
     164             :     }
     165             :     // return success
     166          47 :     return true;
     167             : }
     168             : 
     169          48 : bool AP_Arming_Sub::disarm(const AP_Arming::Method method, bool do_disarm_checks)
     170             : {
     171             :     // return immediately if we are already disarmed
     172          48 :     if (!sub.motors.armed()) {
     173           1 :         return false;
     174             :     }
     175             : 
     176          47 :     if (!AP_Arming::disarm(method, do_disarm_checks)) {
     177           0 :         return false;
     178             :     }
     179             : 
     180          47 :     send_arm_disarm_statustext("Disarming motors");
     181             : 
     182             :     // send disarm command to motors
     183          47 :     sub.motors.armed(false);
     184             : 
     185             :     // reset the mission
     186          47 :     sub.mission.reset();
     187             : 
     188             : #if HAL_LOGGING_ENABLED
     189          47 :     AP::logger().set_vehicle_armed(false);
     190             : #endif
     191             : 
     192          47 :     hal.util->set_soft_armed(false);
     193             : 
     194             :     // clear input holds
     195          47 :     sub.clear_input_hold();
     196             : 
     197          47 :     return true;
     198             : }