5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 15:38:29 -04:00

Plane: move quadplane arming checks to own function and add angle max and tiltrotor / tailsitter checks

This commit is contained in:
Iampete1 2021-11-06 16:42:40 +00:00 committed by Andrew Tridgell
parent 065cb2decb
commit 5ba2bd675a
2 changed files with 71 additions and 35 deletions

View File

@ -75,33 +75,7 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure)
}
#if HAL_QUADPLANE_ENABLED
if (plane.quadplane.enabled() && !plane.quadplane.available()) {
check_failed(display_failure, "Quadplane enabled but not running");
ret = false;
}
if (plane.quadplane.available() && plane.scheduler.get_loop_rate_hz() < 100) {
check_failed(display_failure, "quadplane needs SCHED_LOOP_RATE >= 100");
ret = false;
}
if (plane.quadplane.available() && !plane.quadplane.motors->initialised_ok()) {
check_failed(display_failure, "Quadplane: check motor setup");
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;
}
}
ret &= quadplane_checks(display_failure);
#endif
if (plane.control_mode == &plane.mode_auto && plane.mission.num_commands() <= 1) {
@ -120,14 +94,6 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure)
ret = false;
}
#if HAL_QUADPLANE_ENABLED
if (plane.quadplane.enabled() && ((plane.quadplane.options & QuadPlane::OPTION_ONLY_ARM_IN_QMODE_OR_AUTO) != 0) &&
!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;
}
#endif
if (plane.g2.flight_options & FlightOptions::CENTER_THROTTLE_TRIM){
int16_t trim = plane.channel_throttle->get_radio_trim();
if (trim < 1250 || trim > 1750) {
@ -139,6 +105,74 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure)
return ret;
}
#if HAL_QUADPLANE_ENABLED
bool AP_Arming_Plane::quadplane_checks(bool display_failure)
{
if (!plane.quadplane.enabled()) {
return true;
}
if (!plane.quadplane.available()) {
check_failed(display_failure, "Quadplane enabled but not running");
return false;
}
bool ret = true;
if (plane.scheduler.get_loop_rate_hz() < 100) {
check_failed(display_failure, "quadplane needs SCHED_LOOP_RATE >= 100");
ret = false;
}
if (!plane.quadplane.motors->initialised_ok()) {
check_failed(display_failure, "Quadplane: check motor setup");
ret = false;
}
// lean angle parameter check
if (plane.quadplane.aparm.angle_max < 1000 || plane.quadplane.aparm.angle_max > 8000) {
check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Check Q_ANGLE_MAX");
ret = false;
}
if ((plane.quadplane.tailsitter.enable > 0) && (plane.quadplane.tiltrotor.enable > 0)) {
check_failed(ARMING_CHECK_PARAMETERS, display_failure, "set TAILSIT_ENABLE 0 or TILT_ENABLE 0");
ret = false;
} else {
if ((plane.quadplane.tailsitter.enable > 0) && !plane.quadplane.tailsitter.enabled()) {
check_failed(ARMING_CHECK_PARAMETERS, display_failure, "tailsitter setup not complete, reboot");
ret = false;
}
if ((plane.quadplane.tiltrotor.enable > 0) && !plane.quadplane.tiltrotor.enabled()) {
check_failed(ARMING_CHECK_PARAMETERS, display_failure, "tiltrotor setup not complete, reboot");
ret = false;
}
}
// 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);
ret = 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);
ret = false;
}
if (((plane.quadplane.options & QuadPlane::OPTION_ONLY_ARM_IN_QMODE_OR_AUTO) != 0) &&
!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;
}
return ret;
}
#endif // HAL_QUADPLANE_ENABLED
bool AP_Arming_Plane::ins_checks(bool display_failure)
{
// call parent class checks

View File

@ -33,6 +33,8 @@ public:
protected:
bool ins_checks(bool report) override;
bool quadplane_checks(bool display_failure);
private:
void change_arm_state(void);