Plane: support motor_test for quadplanes

ArduPlane/GCS_Mavlink.cpp

@@ -1539,6 +1539,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             break;
 #endif
 
+        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 = plane.quadplane.mavlink_motor_test_start(chan, (uint8_t)packet.param1, (uint8_t)packet.param2, (uint16_t)packet.param3, packet.param4);
+            break;
+            
         case MAV_CMD_DO_VTOL_TRANSITION:
             result = plane.quadplane.handle_do_vtol_transition(packet);
             break;

ArduPlane/motor_test.cpp

@@ -0,0 +1,118 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include "Plane.h"
+
+/*
+  mavlink motor test - implements the MAV_CMD_DO_MOTOR_TEST mavlink
+                       command so that the quadplane pilot can test an
+                       individual motor to ensure proper wiring, rotation.
+ */
+
+// motor test definitions
+#define MOTOR_TEST_PWM_MIN              800     // min pwm value accepted by the test
+#define MOTOR_TEST_PWM_MAX              2200    // max pwm value accepted by the test
+#define MOTOR_TEST_TIMEOUT_MS_MAX       30000   // max timeout is 30 seconds
+
+// motor_test_output - checks for timeout and sends updates to motors objects
+void QuadPlane::motor_test_output()
+{
+    // exit immediately if the motor test is not running
+    if (!motor_test.running) {
+        return;
+    }
+
+    // check for test timeout
+    if ((AP_HAL::millis() - motor_test.start_ms) >= motor_test.timeout_ms) {
+        // stop motor test
+        motor_test_stop();
+        return;
+    }
+            
+    int16_t pwm = 0;   // pwm that will be output to the motors
+
+    // 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) {
+            pwm = plane.channel_throttle->radio_min + (plane.channel_throttle->radio_max - plane.channel_throttle->radio_min) * (float)motor_test.throttle_value/100.0f;
+        }
+        break;
+
+    case MOTOR_TEST_THROTTLE_PWM:
+        pwm = motor_test.throttle_value;
+        break;
+
+    case MOTOR_TEST_THROTTLE_PILOT:
+        pwm = plane.channel_throttle->radio_in;
+        break;
+
+    default:
+        motor_test_stop();
+        return;
+    }
+
+    // sanity check throttle values
+    if (pwm >= MOTOR_TEST_PWM_MIN && pwm <= MOTOR_TEST_PWM_MAX ) {
+        // turn on motor to specified pwm vlaue
+        motors->output_test(motor_test.seq, pwm);
+    } else {
+        motor_test_stop();
+    }
+}
+
+// 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 QuadPlane::mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_seq, uint8_t throttle_type,
+                                            uint16_t throttle_value, float timeout_sec)
+{
+    if (motors->armed()) {
+        plane.gcs_send_text(MAV_SEVERITY_INFO, "Must be disarmed for motor test");
+        return MAV_RESULT_FAILED;
+    }
+    // if test has not started try to start it
+    if (!motor_test.running) {
+        // start test
+        motor_test.running = true;
+
+        // enable and arm motors
+        set_armed(true);
+        
+        // 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 QuadPlane::motor_test_stop()
+{
+    // exit immediately if the test is not running
+    if (!motor_test.running) {
+        return;
+    }
+
+    // flag test is complete
+    motor_test.running = false;
+
+    // disarm motors
+    set_armed(false);
+
+    // reset timeout
+    motor_test.start_ms = 0;
+    motor_test.timeout_ms = 0;
+
+    // turn off notify leds
+    AP_Notify::flags.esc_calibration = false;
+}

ArduPlane/quadplane.cpp

@@ -335,7 +335,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
     // @Param: FRAME_CLASS
     // @DisplayName: Frame Class
     // @Description: Controls major frame class for multicopter component
-    // @Values: 0:Quad, 1:Hexa, 2:Octa
+    // @Values: 0:Quad, 1:Hexa, 2:Octa, 3:OctaQuad
     // @User: Standard
     AP_GROUPINFO("FRAME_CLASS", 30, QuadPlane, frame_class, 0),
 
@@ -875,6 +875,11 @@ void QuadPlane::update(void)
         return;
     }
 
+    if (motor_test.running) {
+        motor_test_output();
+        return;
+    }
+    
     if (!in_vtol_mode()) {
         update_transition();
     } else {

ArduPlane/quadplane.h

@@ -200,5 +200,22 @@ private:
         FRAME_CLASS_QUAD=0,
         FRAME_CLASS_HEXA=1,
         FRAME_CLASS_OCTA=2,
+        FRAME_CLASS_OCTAQUAD=3,
     };
+
+    struct {
+        bool running;
+        uint32_t start_ms;            // system time the motor test began
+        uint32_t timeout_ms = 0;      // test will timeout this many milliseconds after the motor_test_start_ms
+        uint8_t seq = 0;              // motor sequence number of motor being tested
+        uint8_t throttle_type = 0;    // motor throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through)
+        uint16_t throttle_value = 0;  // throttle to be sent to motor, value depends upon it's type
+    } motor_test;
+
+public:
+    void motor_test_output();
+    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);
+private:
+    void motor_test_stop();
 };