Sub: Create MOTOR_DETECT mode

This mode attempts to detect the direction of all motors and adjust parameters appropriately.

ArduSub/ArduSub.cpp

@@ -103,7 +103,8 @@ void Sub::fast_loop()
     // update INS immediately to get current gyro data populated
     ins.update();
 
-    if (control_mode != MANUAL) { //don't run rate controller in manual mode
+    //don't run rate controller in manual or motordetection modes
+    if (control_mode != MANUAL && control_mode != MOTOR_DETECT) {
         // run low level rate controllers that only require IMU data
         attitude_control.rate_controller_run();
     }

ArduSub/Sub.h

@@ -558,6 +558,9 @@ private:
     bool poshold_init(void);
     void poshold_run();
 
+    bool motordetect_init();
+    void motordetect_run();
+
     bool stabilize_init(void);
     void stabilize_run();
     bool manual_init(void);

ArduSub/control_motordetect.cpp

@@ -0,0 +1,174 @@
+#include "Sub.h"
+#include "stdio.h"
+
+/*
+ * control_motordetect.cpp - init and run calls for motordetect flightmode;
+ *
+ *  This mode pulses all thrusters to detect if they need to be reversed.
+ *  This still requires that the user has the correct frame selected and the motors
+ *  are connected to the correct ESCs.
+ *
+ *  For each motor:
+ *      wait until vehicle is stopped for > 500ms
+ *      apply throttle up for 500ms
+ *      If results are good:
+ *          save direction and try the next motor.
+ *      else
+ *          wait until vehicle is stopped for > 500ms
+ *          apply throttle down for 500ms
+ *          If results are good
+ *              save direction and try the next motor.
+ *          If results are bad
+ *              Abort!
+ */
+
+namespace {
+
+    // controller states
+    enum test_state {
+        STANDBY,
+        SETTLING,
+        THRUSTING,
+        DETECTING,
+        DONE
+    };
+
+    enum direction {
+        UP = 1,
+        DOWN = -1
+    };
+
+    static uint32_t settling_timer;
+    static uint32_t thrusting_timer;
+    static uint8_t md_state;
+    static uint8_t current_motor;
+    static int16_t current_direction;
+}
+
+bool Sub::motordetect_init()
+{
+    current_motor = 0;
+    md_state = STANDBY;
+    current_direction = UP;
+    return true;
+}
+
+void Sub::motordetect_run()
+{
+    // if not armed set throttle to zero and exit immediately
+    if (!motors.armed()) {
+        motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
+        // Force all motors to stop
+        for(uint8_t i=0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
+            if (motors.motor_is_enabled(i)) {
+                motors.output_test_num(i, 1500);
+            }
+        }
+        md_state = STANDBY;
+        return;
+    }
+
+    switch(md_state) {
+    // Motor detection is not running, set it up to start.
+    case STANDBY:
+        current_direction = UP;
+        current_motor = 0;
+        settling_timer = AP_HAL::millis();
+        md_state = SETTLING;
+        break;
+
+    // Wait until sub stays for 500ms not spinning and leveled.
+    case SETTLING:
+        // Force all motors to stop
+        for (uint8_t i=0; i <AP_MOTORS_MAX_NUM_MOTORS; i++) {
+            if (motors.motor_is_enabled(i)) {
+                !motors.output_test_num(i, 1500);
+            }
+        }
+        // wait until gyro product is under a certain(experimental) threshold
+        if ((ahrs.get_gyro()*ahrs.get_gyro()) > 0.01) {
+            settling_timer = AP_HAL::millis();
+        }
+        // then wait 500ms more
+        if (AP_HAL::millis() > (settling_timer + 500)) {
+            md_state = THRUSTING;
+            thrusting_timer = AP_HAL::millis();
+        }
+
+        break;
+
+    // Thrusts motor for 500ms
+    case THRUSTING:
+        if (AP_HAL::millis() < (thrusting_timer + 500)) {
+            if (!motors.output_test_num(current_motor, 1500 + 300*current_direction)) {
+                md_state = DONE;
+            };
+
+        } else {
+            md_state = DETECTING;
+        }
+        break;
+
+    // Checks the result of thrusting the motor.
+    // Starts again at the other direction if unable to get a good reading.
+    // Fails if it is the second reading and it is still not good.
+    // Set things up to test the next motor if the reading is good.
+    case DETECTING:
+    {
+        // This logic results in a vector such as (1, -1, 0)
+        // TODO: make these thresholds parameters
+        Vector3f gyro = ahrs.get_gyro();
+        bool roll_up = gyro.x > 0.4;
+        bool roll_down = gyro.x < -0.4;
+        int roll = (int(roll_up) - int(roll_down))*current_direction;
+
+        bool pitch_up = gyro.y > 0.4;
+        bool pitch_down = gyro.y < -0.4;
+        int pitch = (int(pitch_up) - int(pitch_down))*current_direction;
+
+        bool yaw_up = gyro.z > 0.5;
+        bool yaw_down = gyro.z < -0.5;
+        int yaw = (+int(yaw_up) - int(yaw_down))*current_direction;
+
+        Vector3f directions(roll, pitch, yaw);
+        // Good read, not inverted
+        if (directions == motors.get_motor_angular_factors(current_motor)) {
+            gcs().send_text(MAV_SEVERITY_INFO, "Thruster %d is ok!", current_motor + 1);
+        }
+        // Good read, inverted
+        else if (-directions == motors.get_motor_angular_factors(current_motor)) {
+            gcs().send_text(MAV_SEVERITY_INFO, "Thruster %d is reversed! Saving it!", current_motor + 1);
+            motors.set_reversed(current_motor, true);
+        }
+        // Bad read!
+        else {
+            gcs().send_text(MAV_SEVERITY_INFO, "Bad thrust read, trying to push the other way...");
+            // If we got here, we couldn't identify anything that made sense.
+            // Let's try pushing the thruster the other way, maybe we are in too shallow waters or hit something
+            if (current_direction == DOWN) {
+                // The reading for the second direction was also bad, we failed.
+                gcs().send_text(MAV_SEVERITY_WARNING, "Failed! Please check Thruster %d and frame setup!", current_motor + 1);
+                md_state = DONE;
+                break;
+            }
+            current_direction = DOWN;
+            md_state = SETTLING;
+            break;
+        }
+        // If we got here, we have a decent motor reading
+        md_state = SETTLING;
+        // Test the next motor, if it exists
+        current_motor++;
+        current_direction = UP;
+        if (!motors.motor_is_enabled(current_motor)) {
+            md_state = DONE;
+            gcs().send_text(MAV_SEVERITY_WARNING, "Motor direction detection is complete.");
+        }
+        break;
+    }
+    case DONE:
+        control_mode = prev_control_mode;
+        arming.disarm();
+        break;
+    }
+}

ArduSub/defines.h

@@ -39,7 +39,8 @@ enum control_mode_t {
     CIRCLE =        7,  // automatic circular flight with automatic throttle
     SURFACE =       9,  // automatically return to surface, pilot maintains horizontal control
     POSHOLD =      16,  // automatic position hold with manual override, with automatic throttle
-    MANUAL =       19   // Pass-through input with no stabilization
+    MANUAL =       19,  // Pass-through input with no stabilization
+    MOTOR_DETECT = 20   // Automatically detect motors orientation
 };
 
 enum mode_reason_t {

ArduSub/flight_mode.cpp

@@ -55,6 +55,10 @@ bool Sub::set_mode(control_mode_t mode, mode_reason_t reason)
         success = manual_init();
         break;
 
+    case MOTOR_DETECT:
+        success = motordetect_init();
+        break;
+
     default:
         success = false;
         break;
@@ -138,6 +142,10 @@ void Sub::update_flight_mode()
         manual_run();
         break;
 
+    case MOTOR_DETECT:
+        motordetect_run();
+        break;
+
     default:
         break;
     }

ArduSub/motors.cpp

@@ -9,6 +9,10 @@ void Sub::enable_motor_output()
 // motors_output - send output to motors library which will adjust and send to ESCs and servos
 void Sub::motors_output()
 {
+    // Motor detection mode controls the thrusters directly
+    if (control_mode == MOTOR_DETECT){
+        return;
+    }
     // check if we are performing the motor test
     if (ap.motor_test) {
         verify_motor_test();