ArduPlane/AP_Arming.cpp

   1 /*
   2   additional arming checks for plane
   3  */
   4 #include "AP_Arming.h"
   5 #include "Plane.h"
   6
   7 #include "qautotune.h"
   8
   9 constexpr uint32_t AP_ARMING_DELAY_MS = 2000; // delay from arming to start of motor spoolup
  10
  11 const AP_Param::GroupInfo AP_Arming_Plane::var_info[] = {
  12     // variables from parent vehicle
  13     AP_NESTEDGROUPINFO(AP_Arming, 0),
  14
  15     // index 3 was RUDDER and should not be used
  16
  17 #if AP_PLANE_BLACKBOX_LOGGING
  18     // @Param: BBOX_SPD
  19     // @DisplayName: Blackbox speed
  20     // @Description: This is a 3D GPS speed threshold above which we will force arm the vehicle to start logging. WARNING: This should only be used on a vehicle with no propellers attached to the flight controller and when the flight controller is not in control of the vehicle.
  21     // @Units: m/s
  22     // @Increment: 1
  23     // @Range: 1 20
  24     // @User: Advanced
  25     AP_GROUPINFO("BBOX_SPD", 4, AP_Arming_Plane, blackbox_speed, 5),
  26 #endif // AP_PLANE_BLACKBOX_LOGGING
  27
  28     AP_GROUPEND
  29 };
  30
  31 // expected to return true if the terrain database is required to have
  32 // all data loaded
  33 bool AP_Arming_Plane::terrain_database_required() const
  34 {
  35 #if AP_TERRAIN_AVAILABLE
  36     if (plane.g.terrain_follow) {
  37         return true;
  38     }
  39 #endif
  40     return AP_Arming::terrain_database_required();
  41 }
  42
  43 /*
  44   additional arming checks for plane
  45
  46  */
  47 bool AP_Arming_Plane::pre_arm_checks(bool display_failure)
  48 {
  49     if (armed || require == (uint8_t)Required::NO) {
  50         // if we are already armed or don't need any arming checks
  51         // then skip the checks
  52         return true;
  53     }
  54     //are arming checks disabled?
  55     if (checks_to_perform == 0) {
  56         return mandatory_checks(display_failure);
  57     }
  58     if (hal.util->was_watchdog_armed()) {
  59         // on watchdog reset bypass arming checks to allow for
  60         // in-flight arming if we were armed before the reset. This
  61         // allows a reset on a BVLOS flight to return home if the
  62         // operator can command arming over telemetry
  63         return true;
  64     }
  65
  66     // call parent class checks
  67     bool ret = AP_Arming::pre_arm_checks(display_failure);
  68
  69 #if AP_AIRSPEED_ENABLED
  70     // Check airspeed sensor
  71     ret &= AP_Arming::airspeed_checks(display_failure);
  72 #endif
  73
  74     if (plane.g.fs_timeout_long < plane.g.fs_timeout_short && plane.g.fs_action_short != FS_ACTION_SHORT_DISABLED) {
  75         check_failed(display_failure, "FS_LONG_TIMEOUT < FS_SHORT_TIMEOUT");
  76         ret = false;
  77     }
  78
  79     if (plane.aparm.roll_limit < 3) {
  80         check_failed(display_failure, "ROLL_LIMIT_DEG too small (%.1f)", plane.aparm.roll_limit.get());
  81         ret = false;
  82     }
  83
  84     if (plane.aparm.pitch_limit_max < 3) {
  85         check_failed(display_failure, "PTCH_LIM_MAX_DEG too small (%.1f)", plane.aparm.pitch_limit_max.get());
  86         ret = false;
  87     }
  88
  89     if (plane.aparm.pitch_limit_min > -3) {
  90         check_failed(display_failure, "PTCH_LIM_MIN_DEG too large (%.1f)", plane.aparm.pitch_limit_min.get());
  91         ret = false;
  92     }
  93
  94     if (plane.aparm.airspeed_min < MIN_AIRSPEED_MIN) {
  95         check_failed(display_failure, "AIRSPEED_MIN too low (%i < %i)", plane.aparm.airspeed_min.get(), MIN_AIRSPEED_MIN);
  96         ret = false;
  97     }
  98
  99     if (plane.channel_throttle->get_reverse() &&
 100         Plane::ThrFailsafe(plane.g.throttle_fs_enabled.get()) != Plane::ThrFailsafe::Disabled &&
 101         plane.g.throttle_fs_value <
 102         plane.channel_throttle->get_radio_max()) {
 103         check_failed(display_failure, "Invalid THR_FS_VALUE for rev throttle");
 104         ret = false;
 105     }
 106
 107     ret &= rc_received_if_enabled_check(display_failure);
 108
 109 #if HAL_QUADPLANE_ENABLED
 110     ret &= quadplane_checks(display_failure);
 111 #endif
 112
 113     // check adsb avoidance failsafe
 114     if (plane.failsafe.adsb) {
 115         check_failed(display_failure, "ADSB threat detected");
 116         ret = false;
 117     }
 118
 119     if (plane.flight_option_enabled(FlightOptions::CENTER_THROTTLE_TRIM)){
 120        int16_t trim = plane.channel_throttle->get_radio_trim();
 121        if (trim < 1250 || trim > 1750) {
 122            check_failed(display_failure, "Throttle trim not near center stick(%u)",trim );
 123            ret = false;
 124        }
 125     }
 126
 127     if (plane.mission.get_in_landing_sequence_flag() &&
 128         !plane.mission.starts_with_takeoff_cmd()) {
 129         check_failed(display_failure,"In landing sequence");
 130         ret = false;
 131     }
 132
 133     char failure_msg[50] {};
 134     if (!plane.control_mode->pre_arm_checks(ARRAY_SIZE(failure_msg), failure_msg)) {
 135         check_failed(display_failure, "%s %s", plane.control_mode->name(), failure_msg);
 136         return false;
 137     }
 138
 139     return ret;
 140 }
 141
 142 bool AP_Arming_Plane::mandatory_checks(bool display_failure)
 143 {
 144     bool ret = true;
 145
 146     ret &= rc_received_if_enabled_check(display_failure);
 147
 148     // Call parent class checks
 149     ret &= AP_Arming::mandatory_checks(display_failure);
 150
 151     return ret;
 152 }
 153
 154
 155 #if HAL_QUADPLANE_ENABLED
 156 bool AP_Arming_Plane::quadplane_checks(bool display_failure)
 157 {
 158     if (!plane.quadplane.enabled()) {
 159         return true;
 160     }
 161
 162     if (!plane.quadplane.available()) {
 163         check_failed(display_failure, "Quadplane enabled but not running");
 164         return false;
 165     }
 166
 167     bool ret = true;
 168
 169     if (plane.scheduler.get_loop_rate_hz() < 100) {
 170         check_failed(display_failure, "quadplane needs SCHED_LOOP_RATE >= 100");
 171         ret = false;
 172     }
 173
 174     char failure_msg[50] {};
 175     if (!plane.quadplane.motors->arming_checks(ARRAY_SIZE(failure_msg), failure_msg)) {
 176         check_failed(display_failure, "Motors: %s", failure_msg);
 177         ret = false;
 178     }
 179
 180     // lean angle parameter check
 181     if (plane.quadplane.aparm.angle_max < 1000 || plane.quadplane.aparm.angle_max > 8000) {
 182         check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Check Q_ANGLE_MAX");
 183         ret = false;
 184     }
 185
 186     if ((plane.quadplane.tailsitter.enable > 0) && (plane.quadplane.tiltrotor.enable > 0)) {
 187         check_failed(ARMING_CHECK_PARAMETERS, display_failure, "set TAILSIT_ENABLE 0 or TILT_ENABLE 0");
 188         ret = false;
 189
 190     } else {
 191
 192         if ((plane.quadplane.tailsitter.enable > 0) && !plane.quadplane.tailsitter.enabled()) {
 193             check_failed(ARMING_CHECK_PARAMETERS, display_failure, "tailsitter setup not complete, reboot");
 194             ret = false;
 195         }
 196
 197         if ((plane.quadplane.tiltrotor.enable > 0) && !plane.quadplane.tiltrotor.enabled()) {
 198             check_failed(ARMING_CHECK_PARAMETERS, display_failure, "tiltrotor setup not complete, reboot");
 199             ret = false;
 200         }
 201     }
 202
 203     // ensure controllers are OK with us arming:
 204     if (!plane.quadplane.pos_control->pre_arm_checks("PSC", failure_msg, ARRAY_SIZE(failure_msg))) {
 205         check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Bad parameter: %s", failure_msg);
 206         ret = false;
 207     }
 208     if (!plane.quadplane.attitude_control->pre_arm_checks("ATC", failure_msg, ARRAY_SIZE(failure_msg))) {
 209         check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Bad parameter: %s", failure_msg);
 210         ret = false;
 211     }
 212
 213     /*
 214       Q_ASSIST_SPEED really should be enabled for all quadplanes except tailsitters
 215      */
 216     if (check_enabled(ARMING_CHECK_PARAMETERS) &&
 217         is_zero(plane.quadplane.assist.speed) &&
 218         !plane.quadplane.tailsitter.enabled()) {
 219         check_failed(display_failure,"Q_ASSIST_SPEED is not set");
 220         ret = false;
 221     }
 222
 223     if ((plane.quadplane.tailsitter.enable > 0) && (plane.quadplane.q_fwd_thr_use != QuadPlane::FwdThrUse::OFF)) {
 224         check_failed(ARMING_CHECK_PARAMETERS, display_failure, "set Q_FWD_THR_USE to 0");
 225         ret = false;
 226     }
 227
 228     return ret;
 229 }
 230 #endif // HAL_QUADPLANE_ENABLED
 231
 232 bool AP_Arming_Plane::ins_checks(bool display_failure)
 233 {
 234     // call parent class checks
 235     if (!AP_Arming::ins_checks(display_failure)) {
 236         return false;
 237     }
 238
 239     // additional plane specific checks
 240     if (check_enabled(ARMING_CHECK_INS)) {
 241         char failure_msg[50] = {};
 242         if (!AP::ahrs().pre_arm_check(true, failure_msg, sizeof(failure_msg))) {
 243             check_failed(ARMING_CHECK_INS, display_failure, "AHRS: %s", failure_msg);
 244             return false;
 245         }
 246     }
 247
 248     return true;
 249 }
 250
 251 bool AP_Arming_Plane::arm_checks(AP_Arming::Method method)
 252 {
 253     if (method == AP_Arming::Method::RUDDER) {
 254         const AP_Arming::RudderArming arming_rudder = get_rudder_arming_type();
 255
 256         if (arming_rudder == AP_Arming::RudderArming::IS_DISABLED) {
 257             //parameter disallows rudder arming/disabling
 258
 259             // if we emit a message here then someone doing surface
 260             // checks may be bothered by the message being emitted.
 261             // check_failed(true, "Rudder arming disabled");
 262             return false;
 263         }
 264
 265         // if throttle is not down, then pilot cannot rudder arm/disarm
 266         if (!is_zero(plane.get_throttle_input())){
 267             check_failed(true, "Non-zero throttle");
 268             return false;
 269         }
 270     }
 271
 272     //are arming checks disabled?
 273     if (checks_to_perform == 0) {
 274         return true;
 275     }
 276
 277     if (hal.util->was_watchdog_armed()) {
 278         // on watchdog reset bypass arming checks to allow for
 279         // in-flight arming if we were armed before the reset. This
 280         // allows a reset on a BVLOS flight to return home if the
 281         // operator can command arming over telemetry
 282         gcs().send_text(MAV_SEVERITY_WARNING, "watchdog: Bypassing arming checks");
 283         return true;
 284     }
 285
 286     // call parent class checks
 287     return AP_Arming::arm_checks(method);
 288 }
 289
 290 /*
 291   update HAL soft arm state
 292 */
 293 void AP_Arming_Plane::change_arm_state(void)
 294 {
 295     update_soft_armed();
 296 #if HAL_QUADPLANE_ENABLED
 297     plane.quadplane.set_armed(hal.util->get_soft_armed());
 298 #endif
 299 }
 300
 301 bool AP_Arming_Plane::arm(const AP_Arming::Method method, const bool do_arming_checks)
 302 {
 303     if (!AP_Arming::arm(method, do_arming_checks)) {
 304         return false;
 305     }
 306
 307     if (plane.update_home()) {
 308         // after update_home the home position could still be
 309         // different from the current_loc if the EKF refused the
 310         // resetHeightDatum call. If we are updating home then we want
 311         // to force the home to be the current_loc so relative alt
 312         // takeoffs work correctly
 313         if (plane.ahrs.set_home(plane.current_loc)) {
 314             // update current_loc
 315             plane.update_current_loc();
 316         }
 317     }
 318
 319     change_arm_state();
 320
 321     // rising edge of delay_arming oneshot
 322     delay_arming = true;
 323
 324     send_arm_disarm_statustext("Throttle armed");
 325
 326     return true;
 327 }
 328
 329 /*
 330   disarm motors
 331  */
 332 bool AP_Arming_Plane::disarm(const AP_Arming::Method method, bool do_disarm_checks)
 333 {
 334     if (do_disarm_checks &&
 335         (AP_Arming::method_is_GCS(method) ||
 336          method == AP_Arming::Method::RUDDER)) {
 337         if (plane.is_flying()) {
 338             // don't allow mavlink or rudder disarm while flying
 339             return false;
 340         }
 341     }
 342
 343     if (do_disarm_checks && method == AP_Arming::Method::RUDDER) {
 344         // option must be enabled:
 345         if (get_rudder_arming_type() != AP_Arming::RudderArming::ARMDISARM) {
 346             gcs().send_text(MAV_SEVERITY_INFO, "Rudder disarm: disabled");
 347             return false;
 348         }
 349     }
 350
 351     if (!AP_Arming::disarm(method, do_disarm_checks)) {
 352         return false;
 353     }
 354     if (plane.control_mode != &plane.mode_auto) {
 355         // reset the mission on disarm if we are not in auto
 356         plane.mission.reset();
 357     }
 358
 359     // suppress the throttle in auto-throttle modes
 360     plane.throttle_suppressed = plane.control_mode->does_auto_throttle();
 361
 362     // if no airmode switch assigned, ensure airmode is off:
 363 #if HAL_QUADPLANE_ENABLED
 364     if ((plane.quadplane.air_mode == AirMode::ON) && (rc().find_channel_for_option(RC_Channel::AUX_FUNC::AIRMODE) == nullptr)) {
 365         plane.quadplane.air_mode = AirMode::OFF;
 366     }
 367 #endif
 368
 369     //only log if disarming was successful
 370     change_arm_state();
 371
 372 #if QAUTOTUNE_ENABLED
 373     //save qautotune gains if enabled and success
 374     if (plane.control_mode == &plane.mode_qautotune) {
 375         plane.quadplane.qautotune.save_tuning_gains();
 376     } else {
 377         plane.quadplane.qautotune.reset();
 378     }
 379 #endif
 380
 381     // re-initialize speed variable used in AUTO and GUIDED for
 382     // DO_CHANGE_SPEED commands
 383     plane.new_airspeed_cm = -1;
 384
 385     send_arm_disarm_statustext("Throttle disarmed");
 386
 387     return true;
 388 }
 389
 390 void AP_Arming_Plane::update_soft_armed()
 391 {
 392     bool _armed = is_armed();
 393 #if HAL_QUADPLANE_ENABLED
 394     if (plane.quadplane.motor_test.running){
 395         _armed = true;
 396     }
 397 #endif
 398
 399     hal.util->set_soft_armed(_armed);
 400 #if HAL_LOGGING_ENABLED
 401     AP::logger().set_vehicle_armed(hal.util->get_soft_armed());
 402 #endif
 403
 404     // update delay_arming oneshot
 405     if (delay_arming &&
 406         (AP_HAL::millis() - hal.util->get_last_armed_change() >= AP_ARMING_DELAY_MS)) {
 407
 408         delay_arming = false;
 409     }
 410
 411 #if AP_PLANE_BLACKBOX_LOGGING
 412     if (blackbox_speed > 0) {
 413         const float speed3d = plane.gps.status() >= AP_GPS::GPS_OK_FIX_3D?plane.gps.velocity().length():0;
 414         const uint32_t now = AP_HAL::millis();
 415         if (speed3d > blackbox_speed) {
 416             last_over_3dspeed_ms = now;
 417         }
 418         if (!_armed && speed3d > blackbox_speed) {
 419             // force safety on so we don't run motors
 420             hal.rcout->force_safety_on();
 421             AP_Param::set_by_name("RC_PROTOCOLS", 0);
 422             arm(Method::BLACKBOX, false);
 423             gcs().send_text(MAV_SEVERITY_WARNING, "BlackBox: arming at %.1f m/s", speed3d);
 424         }
 425         if (_armed && now - last_over_3dspeed_ms > 20000U) {
 426             gcs().send_text(MAV_SEVERITY_WARNING, "BlackBox: disarming at %.1f m/s", speed3d);
 427             disarm(Method::BLACKBOX, false);
 428         }
 429     }
 430 #endif
 431 }
 432
 433 /*
 434   extra plane mission checks
 435  */
 436 bool AP_Arming_Plane::mission_checks(bool report)
 437 {
 438     // base checks
 439     bool ret = AP_Arming::mission_checks(report);
 440     if (plane.mission.contains_item(MAV_CMD_DO_LAND_START) && plane.g.rtl_autoland == RtlAutoland::RTL_DISABLE) {
 441         ret = false;
 442         check_failed(ARMING_CHECK_MISSION, report, "DO_LAND_START set and RTL_AUTOLAND disabled");
 443     }
 444 #if HAL_QUADPLANE_ENABLED
 445     if (plane.quadplane.available()) {
 446         const uint16_t num_commands = plane.mission.num_commands();
 447         AP_Mission::Mission_Command prev_cmd {};
 448         for (uint16_t i=1; i<num_commands; i++) {
 449             AP_Mission::Mission_Command cmd;
 450             if (!plane.mission.read_cmd_from_storage(i, cmd)) {
 451                 break;
 452             }
 453             if (plane.is_land_command(cmd.id) &&
 454                 prev_cmd.id == MAV_CMD_NAV_WAYPOINT) {
 455                 const float dist = cmd.content.location.get_distance(prev_cmd.content.location);
 456                 const float tecs_land_speed = plane.TECS_controller.get_land_airspeed();
 457                 const float landing_speed = is_positive(tecs_land_speed)?tecs_land_speed:plane.aparm.airspeed_cruise;
 458                 const float min_dist = 0.75 * plane.quadplane.stopping_distance(sq(landing_speed));
 459                 if (dist < min_dist) {
 460                     ret = false;
 461                     check_failed(ARMING_CHECK_MISSION, report, "VTOL land too short, min %.0fm", min_dist);
 462                 }
 463             }
 464             prev_cmd = cmd;
 465         }
 466     }
 467 #endif
 468     return ret;
 469 }
 470
 471 // Checks rc has been received if it is configured to be used
 472 bool AP_Arming_Plane::rc_received_if_enabled_check(bool display_failure)
 473 {
 474     if (rc().enabled_protocols() == 0) {
 475         // No protocols enabled, will never get RC, don't block arming
 476         return true;
 477     }
 478
 479     // If RC failsafe is enabled we must receive RC before arming
 480     if ((Plane::ThrFailsafe(plane.g.throttle_fs_enabled.get()) == Plane::ThrFailsafe::Enabled) &&
 481         !(rc().has_had_rc_receiver() || rc().has_had_rc_override())) {
 482         check_failed(display_failure, "Waiting for RC");
 483         return false;
 484     }
 485
 486     return true;
 487 }