Line data    Source code

       1             : #include "Copter.h"
       2             : 
       3             : /*
       4             :   mavlink motor test - implements the MAV_CMD_DO_MOTOR_TEST mavlink command so that the GCS/pilot can test an individual motor or flaps
       5             :                        to ensure proper wiring, rotation.
       6             :  */
       7             : 
       8             : // motor test definitions
       9             : #define MOTOR_TEST_TIMEOUT_SEC          600     // max timeout is 10 minutes (600 seconds)
      10             : 
      11             : static uint32_t motor_test_start_ms;        // system time the motor test began
      12             : static uint32_t motor_test_timeout_ms;      // test will timeout this many milliseconds after the motor_test_start_ms
      13             : static uint8_t motor_test_seq;              // motor sequence number of motor being tested
      14             : static uint8_t motor_test_count;            // number of motors to test
      15             : static uint8_t motor_test_throttle_type;    // motor throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through)
      16             : static float motor_test_throttle_value;  // throttle to be sent to motor, value depends upon it's type
      17             : 
      18             : // motor_test_output - checks for timeout and sends updates to motors objects
      19        8001 : void Copter::motor_test_output()
      20             : {
      21             :     // exit immediately if the motor test is not running
      22        8001 :     if (!ap.motor_test) {
      23        1604 :         return;
      24             :     }
      25             : 
      26        8001 :     EXPECT_DELAY_MS(2000);
      27             : 
      28             :     // check for test timeout
      29        8001 :     uint32_t now = AP_HAL::millis();
      30        8001 :     if ((now - motor_test_start_ms) >= motor_test_timeout_ms) {
      31        1608 :         if (motor_test_count > 1) {
      32        1604 :             if (now - motor_test_start_ms < motor_test_timeout_ms*1.5) {
      33             :                 // output zero for 50% of the test time
      34        1600 :                 motors->output_min();
      35             :             } else {
      36             :                 // move onto next motor
      37           4 :                 motor_test_seq++;
      38           4 :                 motor_test_count--;
      39           4 :                 motor_test_start_ms = now;
      40           4 :                 if (!motors->armed()) {
      41           0 :                     motors->armed(true);
      42           0 :                     hal.util->set_soft_armed(true);
      43             :                 }
      44             :             }
      45        1604 :             return;
      46             :         }
      47             :         // stop motor test
      48           4 :         motor_test_stop();
      49             :     } else {
      50        6393 :         int16_t pwm = 0;   // pwm that will be output to the motors
      51             : 
      52             :         // calculate pwm based on throttle type
      53        6393 :         switch (motor_test_throttle_type) {
      54             : 
      55           0 :             case MOTOR_TEST_COMPASS_CAL:
      56           0 :                 compass.set_voltage(battery.voltage());
      57           0 :                 compass.per_motor_calibration_update();
      58             :                 FALLTHROUGH;
      59             : 
      60        3197 :             case MOTOR_TEST_THROTTLE_PERCENT:
      61             :                 // sanity check motor_test_throttle value
      62             : #if FRAME_CONFIG != HELI_FRAME
      63        3197 :                 if (motor_test_throttle_value <= 100) {
      64        3197 :                     int16_t pwm_min = motors->get_pwm_output_min();
      65        3197 :                     int16_t pwm_max = motors->get_pwm_output_max();
      66        3197 :                     pwm = (int16_t) (pwm_min + (pwm_max - pwm_min) * motor_test_throttle_value * 1e-2f);
      67             :                 }
      68             : #endif
      69        3197 :                 break;
      70             : 
      71        3196 :             case MOTOR_TEST_THROTTLE_PWM:
      72        3196 :                 pwm = (int16_t)motor_test_throttle_value;
      73        3196 :                 break;
      74             : 
      75           0 :             case MOTOR_TEST_THROTTLE_PILOT:
      76           0 :                 pwm = channel_throttle->get_radio_in();
      77           0 :                 break;
      78             : 
      79           0 :             default:
      80           0 :                 motor_test_stop();
      81           0 :                 return;
      82             :         }
      83             : 
      84             :         // sanity check throttle values
      85        6393 :         if (pwm < RC_Channel::RC_MIN_LIMIT_PWM || pwm > RC_Channel::RC_MAX_LIMIT_PWM) {
      86           0 :             motor_test_stop();
      87           0 :             return;
      88             :         }
      89             : 
      90             :         // turn on motor to specified pwm value
      91        6393 :         if (!motors->output_test_seq(motor_test_seq, pwm)) {
      92           0 :             gcs().send_text(MAV_SEVERITY_INFO, "Motor Test: cancelled");
      93           0 :             motor_test_stop();
      94             :         }
      95             :     }
      96        8001 : }
      97             : 
      98             : // mavlink_motor_test_check - perform checks before motor tests can begin
      99             : //  return true if tests can continue, false if not
     100           4 : bool Copter::mavlink_motor_control_check(const GCS_MAVLINK &gcs_chan, bool check_rc, const char* mode)
     101             : {
     102             :     // check board has initialised
     103           4 :     if (!ap.initialised) {
     104           0 :         gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"%s: Board initialising", mode);
     105           0 :         return false;
     106             :     }
     107             : 
     108             :     // Check Motor test is allowed
     109           4 :     char failure_msg[100] {};
     110           4 :     if (!motors->motor_test_checks(ARRAY_SIZE(failure_msg), failure_msg)) {
     111           0 :         gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"%s: %s", mode, failure_msg);
     112           0 :         return false;
     113             :     }
     114             : 
     115             :     // check rc has been calibrated
     116           4 :     if (check_rc && !arming.rc_calibration_checks(true)) {
     117           0 :         gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"%s: RC not calibrated", mode);
     118           0 :         return false;
     119             :     }
     120             : 
     121             :     // ensure we are landed
     122           4 :     if (!ap.land_complete) {
     123           0 :         gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"%s: vehicle not landed", mode);
     124           0 :         return false;
     125             :     }
     126             : 
     127             :     // if we got this far the check was successful and the motor test can continue
     128           4 :     return true;
     129             : }
     130             : 
     131             : // mavlink_motor_test_start - start motor test - spin a single motor at a specified pwm
     132             : //  returns MAV_RESULT_ACCEPTED on success, MAV_RESULT_FAILED on failure
     133           4 : MAV_RESULT Copter::mavlink_motor_test_start(const GCS_MAVLINK &gcs_chan, uint8_t motor_seq, uint8_t throttle_type, float throttle_value,
     134             :                                          float timeout_sec, uint8_t motor_count)
     135             : {
     136           4 :     if (motor_count == 0) {
     137           0 :         motor_count = 1;
     138             :     }
     139             :     // if test has not started try to start it
     140           4 :     if (!ap.motor_test) {
     141             :         /* perform checks that it is ok to start test
     142             :            The RC calibrated check can be skipped if direct pwm is
     143             :            supplied
     144             :         */
     145           4 :         if (!mavlink_motor_control_check(gcs_chan, throttle_type != 1, "Motor Test")) {
     146           0 :             return MAV_RESULT_FAILED;
     147             :         } else {
     148             :             // start test
     149           4 :             gcs().send_text(MAV_SEVERITY_INFO, "starting motor test");
     150             : 
     151           4 :             EXPECT_DELAY_MS(3000);
     152             : 
     153             :             // enable and arm motors
     154           4 :             if (!motors->armed()) {
     155           4 :                 motors->output_min();  // output lowest possible value to motors
     156           4 :                 motors->armed(true);
     157           4 :                 hal.util->set_soft_armed(true);
     158             :             }
     159             : 
     160             :             // disable throttle and gps failsafe
     161           4 :             g.failsafe_throttle.set(FS_THR_Action::DISABLED);
     162           4 :             g.failsafe_gcs.set(FS_GCS_Action::DISABLED);
     163           4 :             g.fs_ekf_action.set(FS_EKF_Action::REPORT_ONLY);
     164             : 
     165             :             // turn on notify leds
     166           4 :             AP_Notify::flags.esc_calibration = true;
     167           4 :             ap.motor_test = true;
     168           4 :         }
     169             :     }
     170             : 
     171             :     // set timeout
     172           4 :     motor_test_start_ms = AP_HAL::millis();
     173           4 :     motor_test_timeout_ms = MIN(timeout_sec, MOTOR_TEST_TIMEOUT_SEC) * 1000;
     174             : 
     175             :     // store required output
     176           4 :     motor_test_seq = motor_seq;
     177           4 :     motor_test_count = motor_count;
     178           4 :     motor_test_throttle_type = throttle_type;
     179           4 :     motor_test_throttle_value = throttle_value;
     180             : 
     181           4 :     if (motor_test_throttle_type == MOTOR_TEST_COMPASS_CAL) {
     182           0 :         compass.per_motor_calibration_start();
     183             :     }            
     184             : 
     185             :     // return success
     186           4 :     return MAV_RESULT_ACCEPTED;
     187             : }
     188             : 
     189             : // motor_test_stop - stops the motor test
     190           4 : void Copter::motor_test_stop()
     191             : {
     192             :     // exit immediately if the test is not running
     193           4 :     if (!ap.motor_test) {
     194           0 :         return;
     195             :     }
     196             : 
     197           4 :     gcs().send_text(MAV_SEVERITY_INFO, "finished motor test");    
     198             : 
     199             :     // disarm motors
     200           4 :     motors->armed(false);
     201           4 :     hal.util->set_soft_armed(false);
     202             : 
     203             :     // reset timeout
     204           4 :     motor_test_start_ms = 0;
     205           4 :     motor_test_timeout_ms = 0;
     206             : 
     207             :     // re-enable failsafes
     208           4 :     g.failsafe_throttle.load();
     209           4 :     g.failsafe_gcs.load();
     210           4 :     g.fs_ekf_action.load();
     211             : 
     212           4 :     if (motor_test_throttle_type == MOTOR_TEST_COMPASS_CAL) {
     213           0 :         compass.per_motor_calibration_end();
     214             :     }
     215             : 
     216             :     // turn off notify leds
     217           4 :     AP_Notify::flags.esc_calibration = false;
     218             : 
     219             :     // flag test is complete
     220           4 :     ap.motor_test = false;
     221             : }