Line data    Source code

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