Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-31 13:08:34 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: relax direction detection logic in motor detect flight mode

Browse Source
This commit is contained in:
Willian Galvani 2019-12-18 16:58:40 -03:00 committed by Jacob Walser
parent 35d8f02c83
commit 9fde00ba30
1 changed files with 28 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
ArduSub/control_motordetect.cpp
View File

@ -130,17 +130,38 @@ void Sub::motordetect_run()
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);
// Readings from the motors table
Vector3f motor_directions = motors.get_motor_angular_factors(current_motor);
// Readings we got from the tests
Vector3f read_directions = Vector3f(roll, pitch, yaw);
// Any correct reading counts, any inverted one invalidates the test
int correct_readings = 0;
int inverted_readings = 0;
for (int i=0; i<3; i++) {
// Factor read as 0 does not count
if (fabsf(read_directions[i]) < 0.1f || fabsf(motor_directions[i]) < 0.1f ) {
continue;
} // Correct reading
else if (fabsf(motor_directions[i] - read_directions[i]) < 0.1f) {
correct_readings += 1;
} // Inverted reading
else if (fabsf(motor_directions[i] + read_directions[i]) < 0.1f) {
inverted_readings += 1;
}
}
// Good read, inverted
else if (-directions == motors.get_motor_angular_factors(current_motor)) {
// Detected the motor at the correct direction
if (correct_readings > 0 && inverted_readings == 0) {
gcs().send_text(MAV_SEVERITY_INFO, "Thruster %d is ok! Saving it!", current_motor + 1);
motors.set_reversed(current_motor, false);
}
// Motor detected and inverted
else if (inverted_readings > 0 && correct_readings == 0) {
gcs().send_text(MAV_SEVERITY_INFO, "Thruster %d is reversed! Saving it!", current_motor + 1);
motors.set_reversed(current_motor, true);
}
// Bad read!
// Could not detect motor properly
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.