Line data    Source code

       1             : #include "Sub.h"
       2             : 
       3             : // enable_motor_output() - enable and output lowest possible value to motors
       4         130 : void Sub::enable_motor_output()
       5             : {
       6         130 :     motors.output_min();
       7         130 : }
       8             : 
       9             : // motors_output - send output to motors library which will adjust and send to ESCs and servos
      10     2180683 : void Sub::motors_output()
      11             : {
      12             :     // Sub has no pre-takeoff RPM gate that asserts the spool-up block,
      13             :     // so clear it every cycle before motors.output() runs output_logic().
      14             :     // Without this the block set during ground-idle ramp would never clear
      15             :     // and the spool would stay in GROUND_IDLE.
      16     2180683 :     motors.set_spoolup_block(false);
      17             :     
      18             :     // Motor detection mode controls the thrusters directly
      19     2180683 :     if (control_mode == Mode::Number::MOTOR_DETECT){
      20           0 :         return;
      21             :     }
      22             :     // check if we are performing the motor test
      23     2180683 :     if (ap.motor_test) {
      24           0 :         verify_motor_test();
      25             :     } else {
      26     2180683 :         motors.set_interlock(true);
      27     2180683 :         auto &srv = AP::srv();
      28     2180683 :         srv.cork();
      29     2180683 :         SRV_Channels::calc_pwm();
      30     2180683 :         SRV_Channels::output_ch_all();
      31     2180683 :         motors.output();
      32     2180683 :         srv.push();
      33             :     }
      34             : }
      35             : 
      36             : // Initialize new style motor test
      37             : // Perform checks to see if it is ok to begin the motor test
      38             : // Returns true if motor test has begun
      39           0 : bool Sub::init_motor_test()
      40             : {
      41           0 :     uint32_t tnow = AP_HAL::millis();
      42             : 
      43             :     // Ten second cooldown period required with no do_set_motor requests required
      44             :     // after failure.
      45           0 :     if (tnow < last_do_motor_test_fail_ms + 10000 && last_do_motor_test_fail_ms > 0) {
      46           0 :         gcs().send_text(MAV_SEVERITY_CRITICAL, "10 second cooldown required after motor test");
      47           0 :         return false;
      48             :     }
      49             : 
      50             :     // check if safety switch has been pushed
      51           0 :     if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) {
      52           0 :         gcs().send_text(MAV_SEVERITY_CRITICAL,"Disarm hardware safety switch before testing motors.");
      53           0 :         return false;
      54             :     }
      55             : 
      56             :     // Make sure we are on the ground
      57           0 :     if (!motors.armed()) {
      58           0 :         gcs().send_text(MAV_SEVERITY_WARNING, "Arm motors before testing motors.");
      59           0 :         return false;
      60             :     }
      61             : 
      62           0 :     enable_motor_output(); // set all motor outputs to zero
      63           0 :     ap.motor_test = true;
      64             : 
      65           0 :     return true;
      66             : }
      67             : 
      68             : // Verify new style motor test
      69             : // The motor test will fail if the interval between received
      70             : // MAV_CMD_DO_SET_MOTOR requests exceeds a timeout period
      71             : // Returns true if it is ok to proceed with new style motor test
      72           0 : bool Sub::verify_motor_test()
      73             : {
      74           0 :     bool pass = true;
      75             : 
      76             :     // Require at least 2 Hz incoming do_set_motor requests
      77           0 :     if (AP_HAL::millis() > last_do_motor_test_ms + 500) {
      78           0 :         gcs().send_text(MAV_SEVERITY_INFO, "Motor test timed out!");
      79           0 :         pass = false;
      80             :     }
      81             : 
      82           0 :     if (!pass) {
      83           0 :         ap.motor_test = false;
      84           0 :         AP::arming().disarm(AP_Arming::Method::MOTORTEST);
      85           0 :         last_do_motor_test_fail_ms = AP_HAL::millis();
      86           0 :         return false;
      87             :     }
      88             : 
      89           0 :     return true;
      90             : }
      91             : 
      92           0 : bool Sub::handle_do_motor_test(mavlink_command_int_t command) {
      93           0 :     last_do_motor_test_ms = AP_HAL::millis();
      94             : 
      95             :     // If we are not already testing motors, initialize test
      96             :     static uint32_t tLastInitializationFailed = 0;
      97           0 :     if(!ap.motor_test) {
      98             :         // Do not allow initializations attempt under 2 seconds
      99             :         // If one fails, we need to give the user time to fix the issue
     100             :         // instead of spamming error messages
     101           0 :         if (AP_HAL::millis() > (tLastInitializationFailed + 2000)) {
     102           0 :             if (!init_motor_test()) {
     103           0 :                 gcs().send_text(MAV_SEVERITY_WARNING, "motor test initialization failed!");
     104           0 :                 tLastInitializationFailed = AP_HAL::millis();
     105           0 :                 return false; // init fail
     106             :             }
     107             :         } else {
     108           0 :             return false;
     109             :         }
     110             :     }
     111             : 
     112           0 :     float motor_number = command.param1;
     113           0 :     float throttle_type = command.param2;
     114           0 :     float throttle = command.param3;
     115             :     // float timeout_s = command.param4; // not used
     116             :     // float motor_count = command.param5; // not used
     117           0 :     const uint32_t test_type = command.y;
     118             : 
     119           0 :     if (test_type != MOTOR_TEST_ORDER_BOARD) {
     120           0 :         gcs().send_text(MAV_SEVERITY_WARNING, "bad test type %0.2f", (double)test_type);
     121           0 :         return false; // test type not supported here
     122             :     }
     123             : 
     124           0 :     if (is_equal(throttle_type, (float)MOTOR_TEST_THROTTLE_PILOT)) {
     125           0 :         gcs().send_text(MAV_SEVERITY_WARNING, "bad throttle type %0.2f", (double)throttle_type);
     126             : 
     127           0 :         return false; // throttle type not supported here
     128             :     }
     129             : 
     130           0 :     if (is_equal(throttle_type, (float)MOTOR_TEST_THROTTLE_PWM)) {
     131           0 :         return motors.output_test_num(motor_number, throttle); // true if motor output is set
     132             :     }
     133             : 
     134           0 :     if (is_equal(throttle_type, (float)MOTOR_TEST_THROTTLE_PERCENT)) {
     135           0 :         throttle = constrain_float(throttle, 0.0f, 100.0f);
     136           0 :         throttle = channel_throttle->get_radio_min() + throttle * 0.01f * (channel_throttle->get_radio_max() - channel_throttle->get_radio_min());
     137           0 :         return motors.output_test_num(motor_number, throttle); // true if motor output is set
     138             :     }
     139             : 
     140           0 :     return false;
     141             : }
     142             : 
     143             : 
     144             : // translate wpnav roll/pitch outputs to lateral/forward
     145      496998 : void Sub::translate_wpnav_rp(float &lateral_out, float &forward_out)
     146             : {
     147             :     // get roll and pitch targets in centidegrees
     148      496998 :     int32_t lateral = wp_nav.get_roll();
     149      496998 :     int32_t forward = -wp_nav.get_pitch(); // output is reversed
     150             : 
     151             :     // constrain target forward/lateral values
     152             :     // The outputs of wp_nav.get_roll and get_pitch should already be constrained to these values
     153      496998 :     const float angle_max_cd = attitude_control.lean_angle_max_cd();
     154      496998 :     lateral = constrain_int16(lateral, -angle_max_cd, angle_max_cd);
     155      496998 :     forward = constrain_int16(forward, -angle_max_cd, angle_max_cd);
     156             : 
     157             :     // Normalize
     158      496998 :     lateral_out = (float)lateral/(float)angle_max_cd;
     159      496998 :     forward_out = (float)forward/(float)angle_max_cd;
     160      496998 : }
     161             : 
     162             : // translate wpnav roll/pitch outputs to lateral/forward
     163      127956 : void Sub::translate_circle_nav_rp(float &lateral_out, float &forward_out)
     164             : {
     165             :     // get roll and pitch targets in centidegrees
     166      127956 :     int32_t lateral = circle_nav.get_roll_cd();
     167      127956 :     int32_t forward = -circle_nav.get_pitch_cd(); // output is reversed
     168             : 
     169             :     // constrain target forward/lateral values
     170      127956 :     const float angle_max_cd = attitude_control.lean_angle_max_cd();
     171      127956 :     lateral = constrain_int16(lateral, -angle_max_cd, angle_max_cd);
     172      127956 :     forward = constrain_int16(forward, -angle_max_cd, angle_max_cd);
     173             : 
     174             :     // Normalize
     175      127956 :     lateral_out = (float)lateral/(float)angle_max_cd;
     176      127956 :     forward_out = (float)forward/(float)angle_max_cd;
     177      127956 : }
     178             : 
     179             : // translate pos_control roll/pitch outputs to lateral/forward
     180      145877 : void Sub::translate_pos_control_rp(float &lateral_out, float &forward_out)
     181             : {
     182             :     // get roll and pitch targets in centidegrees
     183      145877 :     int32_t lateral = pos_control.get_roll_cd();
     184      145877 :     int32_t forward = -pos_control.get_pitch_cd(); // output is reversed
     185             : 
     186             :     // constrain target forward/lateral values
     187      145877 :     const float angle_max_cd = attitude_control.lean_angle_max_cd();
     188      145877 :     lateral = constrain_int16(lateral, -angle_max_cd, angle_max_cd);
     189      145877 :     forward = constrain_int16(forward, -angle_max_cd, angle_max_cd);
     190             : 
     191             :     // Normalize
     192      145877 :     lateral_out = (float)lateral/(float)angle_max_cd;
     193      145877 :     forward_out = (float)forward/(float)angle_max_cd;
     194      145877 : }