mirror of https://github.com/ArduPilot/ardupilot
Plane: combine per mode arm and pre-arm checks
This commit is contained in:
parent
dcd55155af
commit
2ffe677d39
|
@ -125,6 +125,12 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure)
|
|||
ret = false;
|
||||
}
|
||||
|
||||
char failure_msg[50] {};
|
||||
if (!plane.control_mode->pre_arm_checks(ARRAY_SIZE(failure_msg), failure_msg)) {
|
||||
check_failed(true, "%s %s", plane.control_mode->name(), failure_msg);
|
||||
return false;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
@ -186,27 +192,6 @@ bool AP_Arming_Plane::quadplane_checks(bool display_failure)
|
|||
ret = false;
|
||||
}
|
||||
|
||||
if (plane.quadplane.option_is_set(QuadPlane::OPTION::ONLY_ARM_IN_QMODE_OR_AUTO)) {
|
||||
if (!plane.control_mode->is_vtol_mode() && (plane.control_mode != &plane.mode_auto) && (plane.control_mode != &plane.mode_guided)) {
|
||||
check_failed(display_failure,"not in Q mode");
|
||||
ret = false;
|
||||
}
|
||||
if ((plane.control_mode == &plane.mode_auto) && !plane.quadplane.is_vtol_takeoff(plane.mission.get_current_nav_cmd().id)) {
|
||||
check_failed(display_failure,"not in VTOL takeoff");
|
||||
ret = false;
|
||||
}
|
||||
}
|
||||
|
||||
if ((plane.control_mode == &plane.mode_auto) && !plane.mission.starts_with_takeoff_cmd()) {
|
||||
check_failed(display_failure,"missing takeoff waypoint");
|
||||
ret = false;
|
||||
}
|
||||
|
||||
if (plane.control_mode == &plane.mode_rtl) {
|
||||
check_failed(display_failure,"in RTL mode");
|
||||
ret = false;
|
||||
}
|
||||
|
||||
/*
|
||||
Q_ASSIST_SPEED really should be enabled for all quadplanes except tailsitters
|
||||
*/
|
||||
|
@ -261,11 +246,6 @@ bool AP_Arming_Plane::arm_checks(AP_Arming::Method method)
|
|||
}
|
||||
}
|
||||
|
||||
if (!plane.control_mode->allows_arming()) {
|
||||
check_failed(true, "Mode does not allow arming");
|
||||
return false;
|
||||
}
|
||||
|
||||
//are arming checks disabled?
|
||||
if (checks_to_perform == 0) {
|
||||
return true;
|
||||
|
|
|
@ -169,3 +169,30 @@ void Mode::update_target_altitude()
|
|||
|
||||
plane.altitude_error_cm = plane.calc_altitude_error_cm();
|
||||
}
|
||||
|
||||
// returns true if the vehicle can be armed in this mode
|
||||
bool Mode::pre_arm_checks(size_t buflen, char *buffer) const
|
||||
{
|
||||
if (!_pre_arm_checks(buflen, buffer)) {
|
||||
if (strlen(buffer) == 0) {
|
||||
// If no message is provided add a generic one
|
||||
hal.util->snprintf(buffer, buflen, "mode not armable");
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// Auto and Guided do not call this to bypass the q-mode check.
|
||||
bool Mode::_pre_arm_checks(size_t buflen, char *buffer) const
|
||||
{
|
||||
#if HAL_QUADPLANE_ENABLED
|
||||
if (plane.quadplane.enabled() && !is_vtol_mode() &&
|
||||
plane.quadplane.option_is_set(QuadPlane::OPTION::ONLY_ARM_IN_QMODE_OR_AUTO)) {
|
||||
hal.util->snprintf(buffer, buflen, "not Q mode");
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
return true;
|
||||
}
|
||||
|
|
|
@ -76,7 +76,7 @@ public:
|
|||
virtual const char *name4() const = 0;
|
||||
|
||||
// returns true if the vehicle can be armed in this mode
|
||||
virtual bool allows_arming() const { return true; }
|
||||
bool pre_arm_checks(size_t buflen, char *buffer) const;
|
||||
|
||||
//
|
||||
// methods that sub classes should override to affect movement of the vehicle in this mode
|
||||
|
@ -132,6 +132,9 @@ protected:
|
|||
// subclasses override this to perform any required cleanup when exiting the mode
|
||||
virtual void _exit() { return; }
|
||||
|
||||
// mode specific pre-arm checks
|
||||
virtual bool _pre_arm_checks(size_t buflen, char *buffer) const;
|
||||
|
||||
#if HAL_QUADPLANE_ENABLED
|
||||
// References for convenience, used by QModes
|
||||
AC_PosControl*& pos_control;
|
||||
|
@ -186,6 +189,7 @@ protected:
|
|||
|
||||
bool _enter() override;
|
||||
void _exit() override;
|
||||
bool _pre_arm_checks(size_t buflen, char *buffer) const override;
|
||||
};
|
||||
|
||||
|
||||
|
@ -238,6 +242,7 @@ public:
|
|||
protected:
|
||||
|
||||
bool _enter() override;
|
||||
bool _pre_arm_checks(size_t buflen, char *buffer) const override { return true; }
|
||||
|
||||
private:
|
||||
float active_radius_m;
|
||||
|
@ -354,6 +359,7 @@ public:
|
|||
protected:
|
||||
|
||||
bool _enter() override;
|
||||
bool _pre_arm_checks(size_t buflen, char *buffer) const override { return false; }
|
||||
|
||||
private:
|
||||
|
||||
|
@ -398,11 +404,13 @@ public:
|
|||
// methods that affect movement of the vehicle in this mode
|
||||
void update() override { }
|
||||
|
||||
bool allows_arming() const override { return false; }
|
||||
|
||||
bool allows_throttle_nudging() const override { return true; }
|
||||
|
||||
bool does_auto_throttle() const override { return true; }
|
||||
|
||||
protected:
|
||||
bool _pre_arm_checks(size_t buflen, char *buffer) const override { return false; }
|
||||
|
||||
};
|
||||
|
||||
class ModeFBWA : public Mode
|
||||
|
@ -591,11 +599,11 @@ public:
|
|||
|
||||
void run() override;
|
||||
|
||||
bool allows_arming() const override { return false; }
|
||||
|
||||
protected:
|
||||
|
||||
bool _enter() override;
|
||||
bool _pre_arm_checks(size_t buflen, char *buffer) const override { return false; }
|
||||
|
||||
};
|
||||
|
||||
class ModeQRTL : public Mode
|
||||
|
@ -613,8 +621,6 @@ public:
|
|||
|
||||
void run() override;
|
||||
|
||||
bool allows_arming() const override { return false; }
|
||||
|
||||
bool does_auto_throttle() const override { return true; }
|
||||
|
||||
void update_target_altitude() override;
|
||||
|
@ -626,6 +632,7 @@ public:
|
|||
protected:
|
||||
|
||||
bool _enter() override;
|
||||
bool _pre_arm_checks(size_t buflen, char *buffer) const override { return false; }
|
||||
|
||||
private:
|
||||
|
||||
|
|
|
@ -139,3 +139,23 @@ bool ModeAuto::does_auto_throttle() const
|
|||
#endif
|
||||
return true;
|
||||
}
|
||||
|
||||
// returns true if the vehicle can be armed in this mode
|
||||
bool ModeAuto::_pre_arm_checks(size_t buflen, char *buffer) const
|
||||
{
|
||||
#if HAL_QUADPLANE_ENABLED
|
||||
if (plane.quadplane.enabled()) {
|
||||
if (plane.quadplane.option_is_set(QuadPlane::OPTION::ONLY_ARM_IN_QMODE_OR_AUTO) &&
|
||||
!plane.quadplane.is_vtol_takeoff(plane.mission.get_current_nav_cmd().id)) {
|
||||
hal.util->snprintf(buffer, buflen, "not in VTOL takeoff");
|
||||
return false;
|
||||
}
|
||||
if (!plane.mission.starts_with_takeoff_cmd()) {
|
||||
hal.util->snprintf(buffer, buflen, "missing takeoff waypoint");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
// Note that this bypasses the base class checks
|
||||
return true;
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue