Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Sub: Create MOTOR_DETECT mode 

This mode attempts to detect the direction of all motors and adjust parameters appropriately.
This commit is contained in:
Willian Galvani 2019-09-23 13:41:30 -03:00 committed by Jacob Walser
parent 9cea7031f5
commit ad9d6e96d9
6 changed files with 193 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduSub/ArduSub.cpp
View File

@ -104,7 +104,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();
}

3
ArduSub/Sub.h
View File

@ -568,6 +568,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);

174
ArduSub/control_motordetect.cpp Normal file
View File

@ -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::DESIRED_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;
}
}

3
ArduSub/defines.h
View File

@ -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 {

8
ArduSub/flight_mode.cpp
View File

@ -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;
@ -137,6 +141,10 @@ void Sub::update_flight_mode()
manual_run();
break;
case MOTOR_DETECT:
motordetect_run();
break;
default:
break;
}

4
ArduSub/motors.cpp
View File

@ -128,6 +128,10 @@ void Sub::init_disarm_motors()
// 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();