AP_Motors: add pre-arm check

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;

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;
+}

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[];
 

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;
+}

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[];
 

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;

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()
     {

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; }