Line data    Source code

       1             : #include "AP_Arming_Rover.h"
       2             : #include "Rover.h"
       3             : 
       4             : // perform pre_arm_rc_checks checks
       5       15190 : bool AP_Arming_Rover::rc_calibration_checks(const bool display_failure)
       6             : {
       7             :     // set rc-checks to success if RC checks are disabled
       8       15190 :     if (!check_enabled(Check::RC)) {
       9           0 :         return true;
      10             :     }
      11             : 
      12             :     const RC_Channel *channels[] = {
      13       15190 :         rover.channel_steer,
      14       15190 :         rover.channel_throttle
      15       15190 :     };
      16       15190 :     const char *channel_names[] = {"Steer", "Throttle"};
      17             : 
      18       45570 :     for (uint8_t i= 0 ; i < ARRAY_SIZE(channels); i++) {
      19       30380 :         const RC_Channel *channel = channels[i];
      20       30380 :         const char *channel_name = channel_names[i];
      21             :         // check if radio has been calibrated
      22       30380 :         if (channel->get_radio_min() > RC_Channel::RC_CALIB_MIN_LIMIT_PWM) {
      23           0 :             check_failed(Check::RC, display_failure, "%s radio min too high", channel_name);
      24           0 :             return false;
      25             :         }
      26       30380 :         if (channel->get_radio_max() < RC_Channel::RC_CALIB_MAX_LIMIT_PWM) {
      27           0 :             check_failed(Check::RC, display_failure, "%s radio max too low", channel_name);
      28           0 :             return false;
      29             :         }
      30             :     }
      31       15190 :     return AP_Arming::rc_calibration_checks(display_failure);
      32             : }
      33             : 
      34             : // performs pre_arm gps related checks and returns true if passed
      35       15226 : bool AP_Arming_Rover::gps_checks(bool display_failure)
      36             : {
      37       15226 :     if (!require_location &&
      38       28483 :         !rover.control_mode->requires_position() &&
      39       13257 :         !rover.control_mode->requires_velocity()) {
      40             :         // we don't care!
      41       13149 :         return true;
      42             :     }
      43             : 
      44             :     // call parent gps checks
      45        2077 :     if (!AP_Arming::gps_checks(display_failure)) {
      46         214 :         return false;
      47             :     }
      48             : 
      49        1863 :     const AP_AHRS &ahrs = AP::ahrs();
      50             : 
      51             :     // always check if inertial nav has started and is ready
      52        1863 :     char failure_msg[50] = {};
      53        1863 :     if (!ahrs.pre_arm_check(true, failure_msg, sizeof(failure_msg))) {
      54           0 :         check_failed(display_failure, "AHRS: %s", failure_msg);
      55           0 :         return false;
      56             :     }
      57             : 
      58             :     // check for ekf failsafe
      59        1863 :     if (rover.failsafe.ekf) {
      60           0 :         check_failed(display_failure, "EKF failsafe");
      61           0 :         return false;
      62             :     }
      63             : 
      64             :     // ensure position estimate is ok
      65        1863 :     if (!rover.ekf_position_ok()) {
      66             :         // vehicle level position estimate checks
      67        1050 :         check_failed(display_failure, "Need Position Estimate");
      68        1050 :         return false;
      69             :     }
      70             : 
      71         813 :     return true;
      72             : }
      73             : 
      74       25250 : bool AP_Arming_Rover::pre_arm_checks(bool report)
      75             : {
      76       25250 :     if (armed) {
      77             :         // if we are already armed then skip the checks
      78       10024 :         return true;
      79             :     }
      80             : 
      81       15226 :     if (!hal.scheduler->is_system_initialized()) {
      82           0 :         check_failed(report, "System not initialised");
      83           0 :         return false;
      84             :     }
      85             : 
      86             :     // are arming checks disabled?
      87       15226 :     if (should_skip_all_checks()) {
      88           0 :         return mandatory_checks(report);
      89             :     }
      90             : 
      91       15226 :     if (rover.g2.sailboat.sail_enabled() && !rover.g2.windvane.enabled()) {
      92           0 :         check_failed(report, "Sailing enabled with no WindVane");
      93           0 :         return false;
      94             :     }
      95             : 
      96             : #pragma clang diagnostic push
      97             : #if defined(__clang_major__) && __clang_major__ >= 14
      98             : #pragma clang diagnostic ignored "-Wbitwise-instead-of-logical"
      99             : #endif
     100       15226 :     return (AP_Arming::pre_arm_checks(report)
     101       15226 :             & motor_checks(report)
     102             : #if AP_OAPATHPLANNER_ENABLED
     103       15226 :             & oa_check(report)
     104             : #endif
     105       15226 :             & parameter_checks(report)
     106       15226 :             & mode_checks(report));
     107             : #pragma clang diagnostic pop
     108             : }
     109             : 
     110         112 : bool AP_Arming_Rover::arm_checks(AP_Arming::Method method)
     111             : {
     112         112 :     if (method == AP_Arming::Method::RUDDER) {
     113             :         // check if arming/disarming allowed from this mode
     114           3 :         if (!rover.control_mode->allows_arming_from_transmitter()) {
     115           0 :             check_failed(true, "Mode not rudder-armable");
     116           0 :             return false;
     117             :         }
     118             :     }
     119             : 
     120             :     // are arming checks disabled?
     121         112 :     if (should_skip_all_checks()) {
     122           0 :         return true;
     123             :     }
     124         112 :     return AP_Arming::arm_checks(method);
     125             : }
     126             : 
     127      629274 : void AP_Arming_Rover::update_soft_armed()
     128             : {
     129      629274 :     hal.util->set_soft_armed(is_armed());
     130      629274 : }
     131             : 
     132             : /*
     133             :   arm motors
     134             :  */
     135         154 : bool AP_Arming_Rover::arm(AP_Arming::Method method, const bool do_arming_checks)
     136             : {
     137         154 :     if (!AP_Arming::arm(method, do_arming_checks)) {
     138          41 :         AP_Notify::events.arming_failed = true;
     139          41 :         return false;
     140             :     }
     141             : 
     142             :     // Set the SmartRTL home location. If activated, SmartRTL will ultimately try to land at this point
     143         113 :     rover.g2.smart_rtl.set_home(true);
     144             : 
     145             :     // initialize simple mode heading
     146         113 :     rover.mode_simple.init_heading();
     147             : 
     148             :     // save home heading for use in sail vehicles
     149         113 :     rover.g2.windvane.record_home_heading();
     150             : 
     151             : #if HAL_LOGGING_ENABLED
     152             :     // Tell logger it can start logging
     153         113 :     AP::logger().set_vehicle_armed(true);
     154             : #endif
     155             : 
     156         113 :     update_soft_armed();
     157             : 
     158         113 :     send_arm_disarm_statustext("Throttle armed");
     159             : 
     160         113 :     return true;
     161             : }
     162             : 
     163             : /*
     164             :   disarm motors
     165             :  */
     166         130 : bool AP_Arming_Rover::disarm(const AP_Arming::Method method, bool do_disarm_checks)
     167             : {
     168         130 :     if (method == AP_Arming::Method::RUDDER) {
     169          16 :         if (rover.g2.motors.active()) {
     170             :             // can't emit a message here as full-rudder while driving
     171             :             // is not uncommon
     172          10 :             return false;
     173             :         }
     174             :     }
     175             : 
     176         120 :     if (!AP_Arming::disarm(method, do_disarm_checks)) {
     177           7 :         return false;
     178             :     }
     179         113 :     if (rover.control_mode != &rover.mode_auto) {
     180             :         // reset the mission on disarm if we are not in auto
     181          96 :         rover.mode_auto.mission.reset();
     182             :     }
     183             : 
     184             : #if HAL_LOGGING_ENABLED
     185             :     // Tell logger it can stop logging
     186         113 :     AP::logger().set_vehicle_armed(false);
     187             : #endif
     188             : 
     189         113 :     update_soft_armed();
     190             : 
     191         113 :     send_arm_disarm_statustext("Throttle disarmed");
     192             : 
     193         113 :     return true;
     194             : }
     195             : 
     196             : #if AP_OAPATHPLANNER_ENABLED
     197             : // check object avoidance has initialised correctly
     198       15226 : bool AP_Arming_Rover::oa_check(bool report)
     199             : {
     200       15226 :     char failure_msg[50] = {};
     201       15226 :     if (rover.g2.oa.pre_arm_check(failure_msg, ARRAY_SIZE(failure_msg))) {
     202       15214 :         return true;
     203             :     }
     204             : 
     205          12 :     check_failed(report, "%s", failure_msg);
     206          12 :     return false;
     207             : }
     208             : #endif  // AP_OAPATHPLANNER_ENABLED
     209             : 
     210             : // perform parameter checks
     211       15226 : bool AP_Arming_Rover::parameter_checks(bool report)
     212             : {
     213             :     // success if parameter checks are disabled
     214       15226 :     if (!check_enabled(Check::PARAMETERS)) {
     215           0 :         return true;
     216             :     }
     217             : 
     218             :     // check waypoint speed is positive
     219       15226 :     if (!is_positive(rover.g2.wp_nav.get_default_speed())) {
     220           0 :         check_failed(Check::PARAMETERS, report, "WP_SPEED too low");
     221           0 :         return false;
     222             :     }
     223             : 
     224       15226 :     return true;
     225             : }
     226             : 
     227             : // check if arming allowed from this mode
     228       15226 : bool AP_Arming_Rover::mode_checks(bool report)
     229             : {
     230             :     //display failure if arming in this mode is not allowed
     231       15226 :     if (!rover.control_mode->allows_arming()) {
     232         112 :         check_failed(report, "Mode not armable");
     233         112 :         return false;
     234             :     }
     235       15114 :     return true;
     236             : }
     237             : 
     238             : // check motors are ready
     239       15226 : bool AP_Arming_Rover::motor_checks(bool report)
     240             : {
     241       15226 :     bool ret = rover.g2.motors.pre_arm_check(report);
     242             : 
     243             : #if HAL_TORQEEDO_ENABLED
     244       15226 :     char failure_msg[50] = {};
     245       15226 :     AP_Torqeedo *torqeedo = AP_Torqeedo::get_singleton();
     246       15226 :     if (torqeedo != nullptr) {
     247       15226 :         if (!torqeedo->pre_arm_checks(failure_msg, ARRAY_SIZE(failure_msg))) {
     248           0 :             check_failed(report, "Torqeedo: %s", failure_msg);
     249           0 :             ret = false;
     250             :         }
     251             :     }
     252             : #endif
     253             : 
     254       15226 :     return ret;
     255             : }