From e72e5a2751cc4465de201e72e737b785fb5b5e4b Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sun, 4 Sep 2022 02:58:37 +0100 Subject: [PATCH] AP_Motors: add pre-arm check --- libraries/AP_Motors/AP_MotorsCoax.h | 3 +++ libraries/AP_Motors/AP_MotorsHeli.cpp | 14 ++++++++++++ libraries/AP_Motors/AP_MotorsHeli.h | 6 ++--- libraries/AP_Motors/AP_MotorsMulticopter.cpp | 23 ++++++++++++++++++++ libraries/AP_Motors/AP_MotorsMulticopter.h | 3 +++ libraries/AP_Motors/AP_MotorsSingle.h | 3 +++ libraries/AP_Motors/AP_Motors_Class.cpp | 14 ++++++++---- libraries/AP_Motors/AP_Motors_Class.h | 1 + 8 files changed, 60 insertions(+), 7 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsCoax.h b/libraries/AP_Motors/AP_MotorsCoax.h index 81ca9551f7..739192145f 100644 --- a/libraries/AP_Motors/AP_MotorsCoax.h +++ b/libraries/AP_Motors/AP_MotorsCoax.h @@ -44,6 +44,9 @@ public: // this can be used to ensure other pwm outputs (i.e. for servos) do not conflict uint32_t get_motor_mask() override; + // Run arming checks + bool arming_checks(size_t buflen, char *buffer) const override { return AP_Motors::arming_checks(buflen, buffer); } + protected: // output - sends commands to the motors void output_armed_stabilizing() override; diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index 523872cf23..b70ec15f88 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -603,3 +603,17 @@ void AP_MotorsHeli::update_turbine_start() } } +bool AP_MotorsHeli::arming_checks(size_t buflen, char *buffer) const +{ + // run base class checks + if (!AP_Motors::arming_checks(buflen, buffer)) { + return false; + } + + if (_heliflags.servo_test_running) { + hal.util->snprintf(buffer, buflen, "Servo Test is still running"); + return false; + } + + return true; +} diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index 2a8f1808bb..b36b461f13 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -143,9 +143,6 @@ public: // set_enable_bailout - allows main code to set when RSC can immediately ramp engine instantly void set_enable_bailout(bool bailout) { _heliflags.enable_bailout = bailout; } - // return true if the servo test is still running/pending - bool servo_test_running() const { return _heliflags.servo_test_running; } - // set land complete flag void set_land_complete(bool landed) { _heliflags.land_complete = landed; } @@ -157,6 +154,9 @@ public: // use leaking integrator management scheme bool using_leaky_integrator() const { return heli_option(HeliOption::USE_LEAKY_I); } + // Run arming checks + bool arming_checks(size_t buflen, char *buffer) const override; + // var_info for holding Parameter information static const struct AP_Param::GroupInfo var_info[]; diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp index 914331e263..d69b62b352 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.cpp +++ b/libraries/AP_Motors/AP_MotorsMulticopter.cpp @@ -799,3 +799,26 @@ void AP_MotorsMulticopter::convert_pwm_min_max_param(int16_t radio_min, int16_t _pwm_min.set_and_save(radio_min); _pwm_max.set_and_save(radio_max); } + +bool AP_MotorsMulticopter::arming_checks(size_t buflen, char *buffer) const +{ + // run base class checks + if (!AP_Motors::arming_checks(buflen, buffer)) { + return false; + } + + // Check output function is setup for each motor + for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) { + if (!motor_enabled[i]) { + continue; + } + uint8_t chan; + SRV_Channel::Aux_servo_function_t function = SRV_Channels::get_motor_function(i); + if (!SRV_Channels::find_channel(function, chan)) { + hal.util->snprintf(buffer, buflen, "no SERVOx_FUNCTION set to Motor%u", i + 1); + return false; + } + } + + return true; +} diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.h b/libraries/AP_Motors/AP_MotorsMulticopter.h index 0c05f04b1c..3246d9586b 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.h +++ b/libraries/AP_Motors/AP_MotorsMulticopter.h @@ -102,6 +102,9 @@ public: // 10hz logging of voltage scaling and max trust void Log_Write() override; + // Run arming checks + bool arming_checks(size_t buflen, char *buffer) const override; + // var_info for holding Parameter information static const struct AP_Param::GroupInfo var_info[]; diff --git a/libraries/AP_Motors/AP_MotorsSingle.h b/libraries/AP_Motors/AP_MotorsSingle.h index bb8f39b2e9..fb5ea19445 100644 --- a/libraries/AP_Motors/AP_MotorsSingle.h +++ b/libraries/AP_Motors/AP_MotorsSingle.h @@ -44,6 +44,9 @@ public: // this can be used to ensure other pwm outputs (i.e. for servos) do not conflict uint32_t get_motor_mask() override; + // Run arming checks + bool arming_checks(size_t buflen, char *buffer) const override { return AP_Motors::arming_checks(buflen, buffer); } + protected: // output - sends commands to the motors void output_armed_stabilizing() override; diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index cc372d1dc6..78932e8b7c 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -197,12 +197,8 @@ void AP_Motors::add_motor_num(int8_t motor_num) { // ensure valid motor number is provided if (motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS) { - uint8_t chan; SRV_Channel::Aux_servo_function_t function = SRV_Channels::get_motor_function(motor_num); SRV_Channels::set_aux_channel_default(function, motor_num); - if (!SRV_Channels::find_channel(function, chan)) { - gcs().send_text(MAV_SEVERITY_ERROR, "Motors: no SERVOx_FUNCTION set to Motor%u", motor_num + 1); - } } } @@ -268,6 +264,16 @@ void AP_Motors::output_test_seq(uint8_t motor_seq, int16_t pwm) } } +bool AP_Motors::arming_checks(size_t buflen, char *buffer) const +{ + if (!initialised_ok()) { + hal.util->snprintf(buffer, buflen, "Check frame class and type"); + return false; + } + + return true; +} + namespace AP { AP_Motors *motors() { diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 567d93a384..690287b8f3 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -109,6 +109,7 @@ public: static AP_Motors *get_singleton(void) { return _singleton; } // check initialisation succeeded + virtual bool arming_checks(size_t buflen, char *buffer) const; bool initialised_ok() const { return _initialised_ok; } void set_initialised_ok(bool val) { _initialised_ok = val; }