2 additional arming checks for plane
9 constexpr uint32_t AP_ARMING_DELAY_MS
= 2000; // delay from arming to start of motor spoolup
11 const AP_Param::GroupInfo
AP_Arming_Plane::var_info
[] = {
12 // variables from parent vehicle
13 AP_NESTEDGROUPINFO(AP_Arming
, 0),
15 // index 3 was RUDDER and should not be used
17 #if AP_PLANE_BLACKBOX_LOGGING
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.
25 AP_GROUPINFO("BBOX_SPD", 4, AP_Arming_Plane
, blackbox_speed
, 5),
26 #endif // AP_PLANE_BLACKBOX_LOGGING
31 // expected to return true if the terrain database is required to have
33 bool AP_Arming_Plane::terrain_database_required() const
35 #if AP_TERRAIN_AVAILABLE
36 if (plane
.g
.terrain_follow
) {
40 return AP_Arming::terrain_database_required();
44 additional arming checks for plane
47 bool AP_Arming_Plane::pre_arm_checks(bool display_failure
)
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
54 //are arming checks disabled?
55 if (checks_to_perform
== 0) {
56 return mandatory_checks(display_failure
);
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
66 // call parent class checks
67 bool ret
= AP_Arming::pre_arm_checks(display_failure
);
69 #if AP_AIRSPEED_ENABLED
70 // Check airspeed sensor
71 ret
&= AP_Arming::airspeed_checks(display_failure
);
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");
79 if (plane
.aparm
.roll_limit
< 3) {
80 check_failed(display_failure
, "ROLL_LIMIT_DEG too small (%.1f)", plane
.aparm
.roll_limit
.get());
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());
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());
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
);
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");
107 ret
&= rc_received_if_enabled_check(display_failure
);
109 #if HAL_QUADPLANE_ENABLED
110 ret
&= quadplane_checks(display_failure
);
113 // check adsb avoidance failsafe
114 if (plane
.failsafe
.adsb
) {
115 check_failed(display_failure
, "ADSB threat detected");
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
);
127 if (plane
.mission
.get_in_landing_sequence_flag() &&
128 !plane
.mission
.starts_with_takeoff_cmd()) {
129 check_failed(display_failure
,"In landing sequence");
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
);
142 bool AP_Arming_Plane::mandatory_checks(bool display_failure
)
146 ret
&= rc_received_if_enabled_check(display_failure
);
148 // Call parent class checks
149 ret
&= AP_Arming::mandatory_checks(display_failure
);
155 #if HAL_QUADPLANE_ENABLED
156 bool AP_Arming_Plane::quadplane_checks(bool display_failure
)
158 if (!plane
.quadplane
.enabled()) {
162 if (!plane
.quadplane
.available()) {
163 check_failed(display_failure
, "Quadplane enabled but not running");
169 if (plane
.scheduler
.get_loop_rate_hz() < 100) {
170 check_failed(display_failure
, "quadplane needs SCHED_LOOP_RATE >= 100");
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
);
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");
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");
192 if ((plane
.quadplane
.tailsitter
.enable
> 0) && !plane
.quadplane
.tailsitter
.enabled()) {
193 check_failed(ARMING_CHECK_PARAMETERS
, display_failure
, "tailsitter setup not complete, reboot");
197 if ((plane
.quadplane
.tiltrotor
.enable
> 0) && !plane
.quadplane
.tiltrotor
.enabled()) {
198 check_failed(ARMING_CHECK_PARAMETERS
, display_failure
, "tiltrotor setup not complete, reboot");
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
);
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
);
214 Q_ASSIST_SPEED really should be enabled for all quadplanes except tailsitters
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");
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");
230 #endif // HAL_QUADPLANE_ENABLED
232 bool AP_Arming_Plane::ins_checks(bool display_failure
)
234 // call parent class checks
235 if (!AP_Arming::ins_checks(display_failure
)) {
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
);
251 bool AP_Arming_Plane::arm_checks(AP_Arming::Method method
)
253 if (method
== AP_Arming::Method::RUDDER
) {
254 const AP_Arming::RudderArming arming_rudder
= get_rudder_arming_type();
256 if (arming_rudder
== AP_Arming::RudderArming::IS_DISABLED
) {
257 //parameter disallows rudder arming/disabling
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");
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");
272 //are arming checks disabled?
273 if (checks_to_perform
== 0) {
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");
286 // call parent class checks
287 return AP_Arming::arm_checks(method
);
291 update HAL soft arm state
293 void AP_Arming_Plane::change_arm_state(void)
296 #if HAL_QUADPLANE_ENABLED
297 plane
.quadplane
.set_armed(hal
.util
->get_soft_armed());
301 bool AP_Arming_Plane::arm(const AP_Arming::Method method
, const bool do_arming_checks
)
303 if (!AP_Arming::arm(method
, do_arming_checks
)) {
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();
321 // rising edge of delay_arming oneshot
324 send_arm_disarm_statustext("Throttle armed");
332 bool AP_Arming_Plane::disarm(const AP_Arming::Method method
, bool do_disarm_checks
)
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
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");
351 if (!AP_Arming::disarm(method
, do_disarm_checks
)) {
354 if (plane
.control_mode
!= &plane
.mode_auto
) {
355 // reset the mission on disarm if we are not in auto
356 plane
.mission
.reset();
359 // suppress the throttle in auto-throttle modes
360 plane
.throttle_suppressed
= plane
.control_mode
->does_auto_throttle();
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
;
369 //only log if disarming was successful
372 #if QAUTOTUNE_ENABLED
373 // Possibly save auto tuned parameters
374 plane
.quadplane
.qautotune
.disarmed(plane
.control_mode
== &plane
.mode_qautotune
);
377 // re-initialize speed variable used in AUTO and GUIDED for
378 // DO_CHANGE_SPEED commands
379 plane
.new_airspeed_cm
= -1;
381 send_arm_disarm_statustext("Throttle disarmed");
386 void AP_Arming_Plane::update_soft_armed()
388 bool _armed
= is_armed();
389 #if HAL_QUADPLANE_ENABLED
390 if (plane
.quadplane
.motor_test
.running
){
395 hal
.util
->set_soft_armed(_armed
);
396 #if HAL_LOGGING_ENABLED
397 AP::logger().set_vehicle_armed(hal
.util
->get_soft_armed());
400 // update delay_arming oneshot
402 (AP_HAL::millis() - hal
.util
->get_last_armed_change() >= AP_ARMING_DELAY_MS
)) {
404 delay_arming
= false;
407 #if AP_PLANE_BLACKBOX_LOGGING
408 if (blackbox_speed
> 0) {
409 const float speed3d
= plane
.gps
.status() >= AP_GPS::GPS_OK_FIX_3D
?plane
.gps
.velocity().length():0;
410 const uint32_t now
= AP_HAL::millis();
411 if (speed3d
> blackbox_speed
) {
412 last_over_3dspeed_ms
= now
;
414 if (!_armed
&& speed3d
> blackbox_speed
) {
415 // force safety on so we don't run motors
416 hal
.rcout
->force_safety_on();
417 AP_Param::set_by_name("RC_PROTOCOLS", 0);
418 arm(Method::BLACKBOX
, false);
419 gcs().send_text(MAV_SEVERITY_WARNING
, "BlackBox: arming at %.1f m/s", speed3d
);
421 if (_armed
&& now
- last_over_3dspeed_ms
> 20000U) {
422 gcs().send_text(MAV_SEVERITY_WARNING
, "BlackBox: disarming at %.1f m/s", speed3d
);
423 disarm(Method::BLACKBOX
, false);
430 extra plane mission checks
432 bool AP_Arming_Plane::mission_checks(bool report
)
435 bool ret
= AP_Arming::mission_checks(report
);
436 if (plane
.mission
.contains_item(MAV_CMD_DO_LAND_START
) && plane
.g
.rtl_autoland
== RtlAutoland::RTL_DISABLE
) {
438 check_failed(ARMING_CHECK_MISSION
, report
, "DO_LAND_START set and RTL_AUTOLAND disabled");
440 #if HAL_QUADPLANE_ENABLED
441 if (plane
.quadplane
.available()) {
442 const uint16_t num_commands
= plane
.mission
.num_commands();
443 AP_Mission::Mission_Command prev_cmd
{};
444 for (uint16_t i
=1; i
<num_commands
; i
++) {
445 AP_Mission::Mission_Command cmd
;
446 if (!plane
.mission
.read_cmd_from_storage(i
, cmd
)) {
449 if (plane
.is_land_command(cmd
.id
) &&
450 prev_cmd
.id
== MAV_CMD_NAV_WAYPOINT
) {
451 const float dist
= cmd
.content
.location
.get_distance(prev_cmd
.content
.location
);
452 const float tecs_land_speed
= plane
.TECS_controller
.get_land_airspeed();
453 const float landing_speed
= is_positive(tecs_land_speed
)?tecs_land_speed
:plane
.aparm
.airspeed_cruise
;
454 const float min_dist
= 0.75 * plane
.quadplane
.stopping_distance(sq(landing_speed
));
455 if (dist
< min_dist
) {
457 check_failed(ARMING_CHECK_MISSION
, report
, "VTOL land too short, min %.0fm", min_dist
);
467 // Checks rc has been received if it is configured to be used
468 bool AP_Arming_Plane::rc_received_if_enabled_check(bool display_failure
)
470 if (rc().enabled_protocols() == 0) {
471 // No protocols enabled, will never get RC, don't block arming
475 // If RC failsafe is enabled we must receive RC before arming
476 if ((Plane::ThrFailsafe(plane
.g
.throttle_fs_enabled
.get()) == Plane::ThrFailsafe::Enabled
) &&
477 !(rc().has_had_rc_receiver() || rc().has_had_rc_override())) {
478 check_failed(display_failure
, "Waiting for RC");