Line data    Source code

       1             : #include "Rover.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             : static const int16_t MOTOR_TEST_PWM_MAX = 2200; // max pwm value accepted by the test
      10             : static const int16_t MOTOR_TEST_TIMEOUT_MS_MAX = 30000; // max timeout is 30 seconds
      11             : 
      12             : static uint32_t motor_test_start_ms = 0;        // system time the motor test began
      13             : static uint32_t motor_test_timeout_ms = 0;      // test will timeout this many milliseconds after the motor_test_start_ms
      14             : static AP_MotorsUGV::motor_test_order motor_test_instance;              // motor instance number of motor being tested (see AP_MotorsUGV::motor_test_order)
      15             : static uint8_t motor_test_throttle_type = 0;    // motor throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through)
      16             : static int16_t motor_test_throttle_value = 0;   // 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         250 : void Rover::motor_test_output()
      20             : {
      21             :     // exit immediately if the motor test is not running
      22         250 :     if (!motor_test) {
      23           0 :         return;
      24             :     }
      25             : 
      26             :     // check for test timeout
      27         250 :     if ((AP_HAL::millis() - motor_test_start_ms) >= motor_test_timeout_ms) {
      28             :         // stop motor test
      29           1 :         motor_test_stop();
      30             :     } else {
      31         249 :         bool test_result = false;
      32             :         // calculate  based on throttle type
      33         249 :         switch (motor_test_throttle_type) {
      34           0 :             case MOTOR_TEST_THROTTLE_PERCENT:
      35           0 :                 test_result = g2.motors.output_test_pct(motor_test_instance, motor_test_throttle_value);
      36           0 :                 break;
      37             : 
      38         249 :             case MOTOR_TEST_THROTTLE_PWM:
      39         249 :                 test_result = g2.motors.output_test_pwm(motor_test_instance, motor_test_throttle_value);
      40         249 :                 break;
      41             : 
      42           0 :             case MOTOR_TEST_THROTTLE_PILOT:
      43           0 :                 if (motor_test_instance == AP_MotorsUGV::MOTOR_TEST_STEERING) {
      44           0 :                     test_result = g2.motors.output_test_pct(motor_test_instance, channel_steer->norm_input_dz() * 100.0f);
      45             :                 } else {
      46           0 :                     test_result = g2.motors.output_test_pct(motor_test_instance, channel_throttle->get_control_in());
      47             :                 }
      48           0 :                 break;
      49             : 
      50           0 :             default:
      51             :                 // do nothing
      52           0 :                 return;
      53             :         }
      54             :         // stop motor test on failure
      55         249 :         if (!test_result) {
      56           0 :             motor_test_stop();
      57             :         }
      58             :     }
      59             : }
      60             : 
      61             : // mavlink_motor_test_check - perform checks before motor tests can begin
      62             : // return true if tests can continue, false if not
      63           1 : bool Rover::mavlink_motor_test_check(const GCS_MAVLINK &gcs_chan, bool check_rc, AP_MotorsUGV::motor_test_order motor_seq, uint8_t throttle_type, int16_t throttle_value)
      64             : {
      65             :     // check board has initialised
      66           1 :     if (!initialised) {
      67           0 :         gcs_chan.send_text(MAV_SEVERITY_CRITICAL, "Motor Test: Board initialising");
      68           0 :         return false;
      69             :     }
      70             : 
      71             :     // check rc has been calibrated
      72           1 :     if (check_rc && !arming.rc_calibration_checks(true)) {
      73           0 :         gcs_chan.send_text(MAV_SEVERITY_CRITICAL, "Motor Test: RC not calibrated");
      74           0 :         return false;
      75             :     }
      76             : 
      77             :     // check if safety switch has been pushed
      78           1 :     if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) {
      79           0 :         gcs_chan.send_text(MAV_SEVERITY_CRITICAL, "Motor Test: Safety switch");
      80           0 :         return false;
      81             :     }
      82             : 
      83             :     // check motor_seq
      84           1 :     if (motor_seq > AP_MotorsUGV::MOTOR_TEST_THROTTLE_RIGHT) {
      85           0 :         gcs_chan.send_text(MAV_SEVERITY_CRITICAL, "Motor Test: invalid motor (%d)", (int)motor_seq);
      86           0 :         return false;
      87             :     }
      88             : 
      89             :     // check throttle type
      90           1 :     if (throttle_type > MOTOR_TEST_THROTTLE_PILOT) {
      91           0 :         gcs_chan.send_text(MAV_SEVERITY_CRITICAL, "Motor Test: invalid throttle type: %d", (int)throttle_type);
      92           0 :         return false;
      93             :     }
      94             : 
      95             :     // check throttle value
      96           1 :     if (throttle_type == MOTOR_TEST_THROTTLE_PWM && throttle_value > MOTOR_TEST_PWM_MAX) {
      97           0 :         gcs_chan.send_text(MAV_SEVERITY_CRITICAL, "Motor Test: pwm (%d) too high", (int)throttle_value);
      98           0 :         return false;
      99             :     }
     100           1 :     if (throttle_type == MOTOR_TEST_THROTTLE_PERCENT && throttle_value > 100) {
     101           0 :         gcs_chan.send_text(MAV_SEVERITY_CRITICAL, "Motor Test: percentage (%d) too high", (int)throttle_value);
     102           0 :         return false;
     103             :     }
     104             : 
     105             :     // if we got this far the check was successful and the motor test can continue
     106           1 :     return true;
     107             : }
     108             : 
     109             : // mavlink_motor_test_start - start motor test - spin a single motor at a specified pwm
     110             : // returns MAV_RESULT_ACCEPTED on success, MAV_RESULT_FAILED on failure
     111           1 : MAV_RESULT Rover::mavlink_motor_test_start(const GCS_MAVLINK &gcs_chan, AP_MotorsUGV::motor_test_order motor_instance, uint8_t throttle_type, int16_t throttle_value, float timeout_sec)
     112             : {
     113             :     // if test has not started try to start it
     114           1 :     if (!motor_test) {
     115             :         /* perform checks that it is ok to start test
     116             :            The RC calibrated check can be skipped if direct pwm is
     117             :            suppliedo
     118             :         */
     119           1 :         if (!mavlink_motor_test_check(gcs_chan, throttle_type != 1, motor_instance, throttle_type, throttle_value)) {
     120           0 :             return MAV_RESULT_FAILED;
     121             :         } else {
     122             :             // start test
     123           1 :             motor_test = true;
     124             : 
     125             :             // arm motors
     126           1 :             if (!arming.is_armed()) {
     127           1 :                 if (!arming.arm(AP_Arming::Method::MOTORTEST)) {
     128           0 :                     return MAV_RESULT_FAILED;
     129             :                 }
     130             :             }
     131             : 
     132             :             // disable failsafes
     133           1 :             g.fs_gcs_enabled.set(0);
     134           1 :             g.fs_throttle_enabled.set(0);
     135           1 :             g.fs_crash_check.set(0);
     136             : 
     137             :             // turn on notify leds
     138           1 :             AP_Notify::flags.esc_calibration = true;
     139             :         }
     140             :     }
     141             : 
     142             :     // set timeout
     143           1 :     motor_test_start_ms = AP_HAL::millis();
     144           1 :     motor_test_timeout_ms = MIN(timeout_sec * 1000, MOTOR_TEST_TIMEOUT_MS_MAX);
     145             : 
     146             :     // store required output
     147           1 :     motor_test_instance = motor_instance;
     148           1 :     motor_test_throttle_type = throttle_type;
     149           1 :     motor_test_throttle_value = throttle_value;
     150             : 
     151             :     // return success
     152           1 :     return MAV_RESULT_ACCEPTED;
     153             : }
     154             : 
     155             : // motor_test_stop - stops the motor test
     156           1 : void Rover::motor_test_stop()
     157             : {
     158             :     // exit immediately if the test is not running
     159           1 :     if (!motor_test) {
     160           0 :         return;
     161             :     }
     162             : 
     163             :     // disarm motors
     164           1 :     AP::arming().disarm(AP_Arming::Method::MOTORTEST);
     165             : 
     166             :     // reset timeout
     167           1 :     motor_test_start_ms = 0;
     168           1 :     motor_test_timeout_ms = 0;
     169             : 
     170             :     // re-enable failsafes
     171           1 :     g.fs_gcs_enabled.load();
     172           1 :     g.fs_throttle_enabled.load();
     173           1 :     g.fs_crash_check.load();
     174             : 
     175             :     // turn off notify leds
     176           1 :     AP_Notify::flags.esc_calibration = false;
     177             : 
     178             :     // flag test is complete
     179           1 :     motor_test = false;
     180             : }