Browse Source

Plane: quadplane use prearm checks from Attitude and Position controllers

master
Peter Barker 6 years ago committed by Andrew Tridgell
parent
commit
cf45b54070
  1. 13
      ArduPlane/AP_Arming.cpp
  2. 1
      ArduPlane/quadplane.h

13
ArduPlane/AP_Arming.cpp

@ -63,6 +63,19 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure) @@ -63,6 +63,19 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure)
ret = false;
}
if (plane.quadplane.enabled() && plane.quadplane.available()) {
// ensure controllers are OK with us arming:
char failure_msg[50];
if (!plane.quadplane.pos_control->pre_arm_checks("PSC", failure_msg, ARRAY_SIZE(failure_msg))) {
check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Bad parameter: %s", failure_msg);
return false;
}
if (!plane.quadplane.attitude_control->pre_arm_checks("ATC", failure_msg, ARRAY_SIZE(failure_msg))) {
check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Bad parameter: %s", failure_msg);
return false;
}
}
if (plane.control_mode == AUTO && plane.mission.num_commands() <= 1) {
check_failed(ARMING_CHECK_NONE, display_failure, "No mission loaded");
ret = false;

1
ArduPlane/quadplane.h

@ -23,6 +23,7 @@ public: @@ -23,6 +23,7 @@ public:
friend class GCS_MAVLINK_Plane;
friend class AP_AdvancedFailsafe_Plane;
friend class QAutoTune;
friend class AP_Arming_Plane;
QuadPlane(AP_AHRS_NavEKF &_ahrs);

Loading…
Cancel
Save