mirror of https://github.com/ArduPilot/ardupilot
Sub: Create MOTOR_DETECT mode
This mode attempts to detect the direction of all motors and adjust parameters appropriately.
This commit is contained in:
parent
dd97eedde3
commit
2b0773c18b
|
@ -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();
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
}
|
|
@ -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 {
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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();
|
||||
|
|
Loading…
Reference in New Issue