Rover: add motor test

APMrover2/GCS_Mavlink.cpp

@@ -1039,6 +1039,16 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
             }
             break;
 
+        case MAV_CMD_DO_MOTOR_TEST:
+            // param1 : motor sequence number (a number from 1 to max number of motors on the vehicle)
+            // param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum)
+            // param3 : throttle (range depends upon param2)
+            // param4 : timeout (in seconds)
+            result = rover.mavlink_motor_test_start(chan, static_cast<uint8_t>(packet.param1),
+                                                    static_cast<uint8_t>(packet.param2),
+                                                    static_cast<uint16_t>(packet.param3),
+                                                    packet.param4);
+            break;
 
         default:
             result = handle_command_long_message(packet);

APMrover2/Rover.h

@@ -419,6 +419,9 @@ private:
     // last visual odometry update time
     uint32_t visual_odom_last_update_ms;
 
+    // True when we are doing motor test
+    bool motor_test;
+
 private:
     // private member functions
     void ahrs_update();
@@ -630,6 +633,11 @@ public:
 
     void dataflash_periodic(void);
     void update_soft_armed();
+    // Motor test
+    void motor_test_output();
+    bool mavlink_motor_test_check(mavlink_channel_t chan, bool check_rc);
+    uint8_t mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_seq, uint8_t throttle_type, uint16_t throttle_value, float timeout_sec);
+    void motor_test_stop();
 };
 
 #define MENU_FUNC(func) FUNCTOR_BIND(&rover, &Rover::func, int8_t, uint8_t, const Menu::arg *)

APMrover2/Steering.cpp

@@ -254,5 +254,10 @@ void Rover::set_servos(void) {
     }
 
     // send output signals to motors
-    g2.motors.output(arming.is_armed() && hal.util->get_soft_armed(), G_Dt);
+    if (motor_test) {
+        g2.motors.slew_limit_throttle(false);
+        motor_test_output();
+    } else {
+        g2.motors.output(arming.is_armed() && hal.util->get_soft_armed(), G_Dt);
+    }
 }

APMrover2/motor_test.cpp

@@ -0,0 +1,158 @@
+#include "Rover.h"
+
+/*
+  mavlink motor test - implements the MAV_CMD_DO_MOTOR_TEST mavlink command so that the GCS/pilot can test an individual motor or flaps
+                       to ensure proper wiring, rotation.
+ */
+
+// motor test definitions
+static const int16_t MOTOR_TEST_PWM_MIN = 1000;     // min pwm value accepted by the test
+static const int16_t MOTOR_TEST_PWM_MAX = 2000;     // max pwm value accepted by the test
+static const int16_t MOTOR_TEST_TIMEOUT_MS_MAX = 30000;   // max timeout is 30 seconds
+
+static uint32_t motor_test_start_ms = 0;        // system time the motor test began
+static uint32_t motor_test_timeout_ms = 0;      // test will timeout this many milliseconds after the motor_test_start_ms
+static uint8_t motor_test_seq = 0;              // motor sequence number of motor being tested
+static uint8_t motor_test_throttle_type = 0;    // motor throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through)
+static uint16_t motor_test_throttle_value = 0;  // throttle to be sent to motor, value depends upon it's type
+
+// motor_test_output - checks for timeout and sends updates to motors objects
+void Rover::motor_test_output()
+{
+    // exit immediately if the motor test is not running
+    if (!motor_test) {
+        return;
+    }
+
+    // check for test timeout
+    if ((AP_HAL::millis() - motor_test_start_ms) >= motor_test_timeout_ms) {
+        // stop motor test
+        motor_test_stop();
+    } else {
+        // calculate pwm based on throttle type
+        switch (motor_test_throttle_type) {
+            case MOTOR_TEST_THROTTLE_PERCENT:
+                // sanity check motor_test_throttle value
+                if (motor_test_throttle_value <= 100) {
+                    g2.motors.set_throttle(motor_test_throttle_value);
+                }
+                break;
+
+            case MOTOR_TEST_THROTTLE_PWM:
+                channel_throttle->set_pwm(motor_test_throttle_value);
+                g2.motors.set_throttle(channel_throttle->get_control_in());
+                break;
+
+            case MOTOR_TEST_THROTTLE_PILOT:
+                g2.motors.set_throttle(channel_throttle->get_control_in());
+                break;
+
+            default:
+                motor_test_stop();
+                return;
+        }
+        const bool test_result = g2.motors.output_test((AP_MotorsUGV::motor_test_order)motor_test_seq);
+        if (!test_result) {
+            motor_test_stop();
+        }
+    }
+}
+
+// mavlink_motor_test_check - perform checks before motor tests can begin
+// return true if tests can continue, false if not
+bool Rover::mavlink_motor_test_check(mavlink_channel_t chan, bool check_rc)
+{
+    GCS_MAVLINK_Rover &gcs_chan = gcs().chan(chan-MAVLINK_COMM_0);
+
+    // check board has initialised
+    if (!initialised) {
+        gcs_chan.send_text(MAV_SEVERITY_CRITICAL, "Motor Test: Board initialising");
+        return false;
+    }
+
+    // check rc has been calibrated
+    if (check_rc && !arming.pre_arm_rc_checks(true)) {
+        gcs_chan.send_text(MAV_SEVERITY_CRITICAL, "Motor Test: RC not calibrated");
+        return false;
+    }
+
+    // check if safety switch has been pushed
+    if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) {
+        gcs_chan.send_text(MAV_SEVERITY_CRITICAL, "Motor Test: Safety switch");
+        return false;
+    }
+
+    // if we got this far the check was successful and the motor test can continue
+    return true;
+}
+
+// mavlink_motor_test_start - start motor test - spin a single motor at a specified pwm
+// returns MAV_RESULT_ACCEPTED on success, MAV_RESULT_FAILED on failure
+uint8_t Rover::mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_seq, uint8_t throttle_type, uint16_t throttle_value, float timeout_sec)
+{
+    // if test has not started try to start it
+    if (!motor_test) {
+        /* perform checks that it is ok to start test
+           The RC calibrated check can be skipped if direct pwm is
+           supplied
+        */
+        if (!mavlink_motor_test_check(chan, throttle_type != 1)) {
+            return MAV_RESULT_FAILED;
+        } else {
+            // start test
+            motor_test = true;
+
+            // arm motors
+            if (!arming.is_armed()) {
+                arm_motors(AP_Arming::NONE);
+            }
+
+            // disable failsafes
+            g.fs_gcs_enabled = 0;
+            g.fs_throttle_enabled = 0;
+            g.fs_crash_check = 0;
+
+            // turn on notify leds
+            AP_Notify::flags.esc_calibration = true;
+        }
+    }
+
+    // set timeout
+    motor_test_start_ms = AP_HAL::millis();
+    motor_test_timeout_ms = MIN(timeout_sec * 1000, MOTOR_TEST_TIMEOUT_MS_MAX);
+
+    // store required output
+    motor_test_seq = motor_seq;
+    motor_test_throttle_type = throttle_type;
+    motor_test_throttle_value = throttle_value;
+
+    // return success
+    return MAV_RESULT_ACCEPTED;
+}
+
+// motor_test_stop - stops the motor test
+void Rover::motor_test_stop()
+{
+    // exit immediately if the test is not running
+    if (!motor_test) {
+        return;
+    }
+
+    // disarm motors
+    disarm_motors();
+
+    // reset timeout
+    motor_test_start_ms = 0;
+    motor_test_timeout_ms = 0;
+
+    // re-enable failsafes
+    g.fs_gcs_enabled.load();
+    g.fs_throttle_enabled.load();
+    g.fs_crash_check.load();
+
+    // turn off notify leds
+    AP_Notify::flags.esc_calibration = false;
+
+    // flag test is complete
+    motor_test = false;
+}