ArduPlane/motor_test.cpp

   1 #include "Plane.h"
   2
   3 /*
   4   mavlink motor test - implements the MAV_CMD_DO_MOTOR_TEST mavlink
   5                        command so that the quadplane pilot can test an
   6                        individual motor to ensure proper wiring, rotation.
   7  */
   8
   9 // motor test definitions
  10 #define MOTOR_TEST_TIMEOUT_MS_MAX       30000   // max timeout is 30 seconds
  11
  12 // motor_test_output - checks for timeout and sends updates to motors objects
  13 #if HAL_QUADPLANE_ENABLED
  14 void QuadPlane::motor_test_output()
  15 {
  16     // exit immediately if the motor test is not running
  17     if (!motor_test.running) {
  18         return;
  19     }
  20
  21     // check for test timeout
  22     uint32_t now = AP_HAL::millis();
  23     if ((now - motor_test.start_ms) >= motor_test.timeout_ms) {
  24         if (motor_test.motor_count > 1) {
  25             if (now - motor_test.start_ms < motor_test.timeout_ms*1.5) {
  26                 // output zero for 0.5s
  27                 motors->output_min();
  28             } else {
  29                 // move onto next motor
  30                 motor_test.seq++;
  31                 motor_test.motor_count--;
  32                 motor_test.start_ms = now;
  33             }
  34             return;
  35         }
  36         // stop motor test
  37         motor_test_stop();
  38         return;
  39     }
  40
  41     int16_t pwm = 0;   // pwm that will be output to the motors
  42
  43     // calculate pwm based on throttle type
  44     const int16_t thr_min_pwm = motors->get_pwm_output_min();
  45     const int16_t thr_max_pwm = motors->get_pwm_output_max();
  46
  47     switch (motor_test.throttle_type) {
  48     case MOTOR_TEST_THROTTLE_PERCENT:
  49         // sanity check motor_test.throttle value
  50         if (motor_test.throttle_value <= 100) {
  51             pwm = thr_min_pwm + (thr_max_pwm - thr_min_pwm) * (float)motor_test.throttle_value*0.01f;
  52         }
  53         break;
  54
  55     case MOTOR_TEST_THROTTLE_PWM:
  56         pwm = motor_test.throttle_value;
  57         break;
  58
  59     case MOTOR_TEST_THROTTLE_PILOT:
  60         pwm = thr_min_pwm + (thr_max_pwm - thr_min_pwm) * plane.get_throttle_input()*0.01f;
  61         break;
  62
  63     default:
  64         motor_test_stop();
  65         return;
  66     }
  67
  68     // sanity check throttle values
  69     if (pwm >= RC_Channel::RC_MIN_LIMIT_PWM && pwm <= RC_Channel::RC_MAX_LIMIT_PWM) {
  70         // turn on motor to specified pwm value
  71         motors->output_test_seq(motor_test.seq, pwm);
  72     } else {
  73         motor_test_stop();
  74     }
  75 }
  76
  77 // mavlink_motor_test_start - start motor test - spin a single motor at a specified pwm
  78 //  returns MAV_RESULT_ACCEPTED on success, MAV_RESULT_FAILED on failure
  79 MAV_RESULT QuadPlane::mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_seq, uint8_t throttle_type,
  80                                             uint16_t throttle_value, float timeout_sec, uint8_t motor_count)
  81 {
  82     if (!available() || motors == nullptr) {
  83         return MAV_RESULT_FAILED;
  84     }
  85
  86     if (motors->armed()) {
  87         gcs().send_text(MAV_SEVERITY_INFO, "Must be disarmed for motor test");
  88         return MAV_RESULT_FAILED;
  89     }
  90
  91     // Check Motor test is allowed
  92     char failure_msg[50] {};
  93     if (!motors->motor_test_checks(ARRAY_SIZE(failure_msg), failure_msg)) {
  94         gcs().send_text(MAV_SEVERITY_CRITICAL,"Motor Test: %s", failure_msg);
  95         return MAV_RESULT_FAILED;
  96     }
  97
  98     // if test has not started try to start it
  99     if (!motor_test.running) {
 100         // start test
 101         motor_test.running = true;
 102
 103         // enable and arm motors
 104         set_armed(true);
 105
 106         // turn on notify leds
 107         AP_Notify::flags.esc_calibration = true;
 108     }
 109
 110     // set timeout
 111     motor_test.start_ms = AP_HAL::millis();
 112     motor_test.timeout_ms = MIN(timeout_sec * 1000, MOTOR_TEST_TIMEOUT_MS_MAX);
 113
 114     // store required output
 115     motor_test.seq = motor_seq;
 116     motor_test.throttle_type = throttle_type;
 117     motor_test.throttle_value = throttle_value;
 118     motor_test.motor_count = MIN(motor_count, 8);
 119
 120     // return success
 121     return MAV_RESULT_ACCEPTED;
 122 }
 123
 124 // motor_test_stop - stops the motor test
 125 void QuadPlane::motor_test_stop()
 126 {
 127     // exit immediately if the test is not running
 128     if (!motor_test.running) {
 129         return;
 130     }
 131
 132     // flag test is complete
 133     motor_test.running = false;
 134
 135     // disarm motors
 136     set_armed(false);
 137
 138     // reset timeout
 139     motor_test.start_ms = 0;
 140     motor_test.timeout_ms = 0;
 141
 142     // turn off notify leds
 143     AP_Notify::flags.esc_calibration = false;
 144 }
 145
 146 #endif  // HAL_QUADPLANE_ENABLED