Line data    Source code

       1             : #include "Sub.h"
       2             : #include "stdio.h"
       3             : 
       4             : /*
       5             :  * control_motordetect.cpp - init and run calls for motordetect flightmode;
       6             :  *
       7             :  *  This mode pulses all thrusters to detect if they need to be reversed.
       8             :  *  This still requires that the user has the correct frame selected and the motors
       9             :  *  are connected to the correct ESCs.
      10             :  *
      11             :  *  For each motor:
      12             :  *      wait until vehicle is stopped for > 500ms
      13             :  *      apply throttle up for 500ms
      14             :  *      If results are good:
      15             :  *          save direction and try the next motor.
      16             :  *      else
      17             :  *          wait until vehicle is stopped for > 500ms
      18             :  *          apply throttle down for 500ms
      19             :  *          If results are good
      20             :  *              save direction and try the next motor.
      21             :  *          If results are bad
      22             :  *              Abort!
      23             :  */
      24             : 
      25             : namespace {
      26             : 
      27             :     // controller states
      28             :     enum test_state {
      29             :         STANDBY,
      30             :         SETTLING,
      31             :         THRUSTING,
      32             :         DETECTING,
      33             :         DONE
      34             :     };
      35             : 
      36             :     enum direction {
      37             :         UP = 1,
      38             :         DOWN = -1
      39             :     };
      40             : 
      41             :     static uint32_t settling_timer;
      42             :     static uint32_t thrusting_timer;
      43             :     static uint8_t md_state;
      44             :     static uint8_t current_motor;
      45             :     static int16_t current_direction;
      46             : }
      47             : 
      48           0 : bool ModeMotordetect::init(bool ignore_checks)
      49             : {
      50           0 :     current_motor = 0;
      51           0 :     md_state = STANDBY;
      52           0 :     current_direction = UP;
      53           0 :     return true;
      54             : }
      55             : 
      56           0 : void ModeMotordetect::run()
      57             : {
      58             :     // if not armed set throttle to zero and exit immediately
      59           0 :     if (!motors.armed()) {
      60           0 :         motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
      61             :         // Force all motors to stop
      62           0 :         for(uint8_t i=0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
      63           0 :             if (motors.motor_is_enabled(i)) {
      64           0 :                 motors.output_test_num(i, 1500);
      65             :             }
      66             :         }
      67           0 :         md_state = STANDBY;
      68           0 :         return;
      69             :     }
      70             : 
      71           0 :     switch(md_state) {
      72             :     // Motor detection is not running, set it up to start.
      73           0 :     case STANDBY:
      74           0 :         current_direction = UP;
      75           0 :         current_motor = 0;
      76           0 :         settling_timer = AP_HAL::millis();
      77           0 :         md_state = SETTLING;
      78           0 :         break;
      79             : 
      80             :     // Wait until sub stays for 500ms not spinning and leveled.
      81           0 :     case SETTLING:
      82             :         // Force all motors to stop
      83           0 :         for (uint8_t i=0; i <AP_MOTORS_MAX_NUM_MOTORS; i++) {
      84           0 :             if (motors.motor_is_enabled(i)) {
      85           0 :                 motors.output_test_num(i, 1500);
      86             :             }
      87             :         }
      88             :         // wait until gyro product is under a certain(experimental) threshold
      89           0 :         if ((ahrs.get_gyro()*ahrs.get_gyro()) > 0.01) {
      90           0 :             settling_timer = AP_HAL::millis();
      91             :         }
      92             :         // then wait 500ms more
      93           0 :         if (AP_HAL::millis() > (settling_timer + 500)) {
      94           0 :             md_state = THRUSTING;
      95           0 :             thrusting_timer = AP_HAL::millis();
      96             :         }
      97             : 
      98           0 :         break;
      99             : 
     100             :     // Thrusts motor for 500ms
     101           0 :     case THRUSTING:
     102           0 :         if (AP_HAL::millis() < (thrusting_timer + 500)) {
     103           0 :             if (!motors.output_test_num(current_motor, 1500 + 300*current_direction)) {
     104           0 :                 md_state = DONE;
     105             :             };
     106             : 
     107             :         } else {
     108           0 :             md_state = DETECTING;
     109             :         }
     110           0 :         break;
     111             : 
     112             :     // Checks the result of thrusting the motor.
     113             :     // Starts again at the other direction if unable to get a good reading.
     114             :     // Fails if it is the second reading and it is still not good.
     115             :     // Set things up to test the next motor if the reading is good.
     116           0 :     case DETECTING:
     117             :     {
     118             :         // This logic results in a vector such as (1, -1, 0)
     119             :         // TODO: make these thresholds parameters
     120           0 :         Vector3f gyro = ahrs.get_gyro();
     121           0 :         bool roll_up = gyro.x > 0.4;
     122           0 :         bool roll_down = gyro.x < -0.4;
     123           0 :         int roll = (int(roll_up) - int(roll_down))*current_direction;
     124             : 
     125           0 :         bool pitch_up = gyro.y > 0.4;
     126           0 :         bool pitch_down = gyro.y < -0.4;
     127           0 :         int pitch = (int(pitch_up) - int(pitch_down))*current_direction;
     128             : 
     129           0 :         bool yaw_up = gyro.z > 0.5;
     130           0 :         bool yaw_down = gyro.z < -0.5;
     131           0 :         int yaw = (+int(yaw_up) - int(yaw_down))*current_direction;
     132             : 
     133           0 :         Vector3f directions(roll, pitch, yaw);
     134             :         // Good read, not inverted
     135           0 :         if (directions == motors.get_motor_angular_factors(current_motor)) {
     136           0 :             gcs().send_text(MAV_SEVERITY_INFO, "Thruster %d is ok!", current_motor + 1);
     137             :         }
     138             :         // Good read, inverted
     139           0 :         else if (-directions == motors.get_motor_angular_factors(current_motor)) {
     140           0 :             gcs().send_text(MAV_SEVERITY_INFO, "Thruster %d is reversed! Saving it!", current_motor + 1);
     141           0 :             motors.set_reversed(current_motor, true);
     142             :         }
     143             :         // Bad read!
     144             :         else {
     145           0 :             gcs().send_text(MAV_SEVERITY_INFO, "Bad thrust read, trying to push the other way...");
     146             :             // If we got here, we couldn't identify anything that made sense.
     147             :             // Let's try pushing the thruster the other way, maybe we are in too shallow waters or hit something
     148           0 :             if (current_direction == DOWN) {
     149             :                 // The reading for the second direction was also bad, we failed.
     150           0 :                 gcs().send_text(MAV_SEVERITY_WARNING, "Failed! Please check Thruster %d and frame setup!", current_motor + 1);
     151           0 :                 md_state = DONE;
     152           0 :                 break;
     153             :             }
     154           0 :             current_direction = DOWN;
     155           0 :             md_state = SETTLING;
     156           0 :             break;
     157             :         }
     158             :         // If we got here, we have a decent motor reading
     159           0 :         md_state = SETTLING;
     160             :         // Test the next motor, if it exists
     161           0 :         current_motor++;
     162           0 :         current_direction = UP;
     163           0 :         if (!motors.motor_is_enabled(current_motor)) {
     164           0 :             md_state = DONE;
     165           0 :             gcs().send_text(MAV_SEVERITY_WARNING, "Motor direction detection is complete.");
     166             :         }
     167           0 :         break;
     168             :     }
     169           0 :     case DONE:
     170           0 :         set_mode(sub.prev_control_mode, ModeReason::MISSION_END);
     171           0 :         sub.arming.disarm(AP_Arming::Method::MOTORDETECTDONE);
     172           0 :         break;
     173             :     }
     174             : }