From 3aca61cefb2cb8e4728c1c6f1be6ed746f9de5c5 Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Mon, 17 Dec 2012 15:28:42 +0900 Subject: [PATCH] AP_Motors: remove unnecessary opposite_motor array (no longer needed with new stability patch) --- libraries/AP_Motors/AP_MotorsHexa.cpp | 24 ++++++------ libraries/AP_Motors/AP_MotorsMatrix.cpp | 29 +------------- libraries/AP_Motors/AP_MotorsMatrix.h | 5 +-- libraries/AP_Motors/AP_MotorsOcta.cpp | 48 +++++++++++------------ libraries/AP_Motors/AP_MotorsOctaQuad.cpp | 32 +++++++-------- libraries/AP_Motors/AP_MotorsQuad.cpp | 16 ++++---- libraries/AP_Motors/AP_MotorsY6.cpp | 12 +++--- 7 files changed, 70 insertions(+), 96 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHexa.cpp b/libraries/AP_Motors/AP_MotorsHexa.cpp index fda15cea7d..46d84b1c57 100644 --- a/libraries/AP_Motors/AP_MotorsHexa.cpp +++ b/libraries/AP_Motors/AP_MotorsHexa.cpp @@ -19,19 +19,19 @@ void AP_MotorsHexa::setup_motors() // hard coded config for supported frames if( _frame_orientation == AP_MOTORS_PLUS_FRAME ) { // plus frame set-up - add_motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_2, 1); - add_motor(AP_MOTORS_MOT_2, 180, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_1, 4); - add_motor(AP_MOTORS_MOT_3,-120, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_4, 5); - add_motor(AP_MOTORS_MOT_4, 60, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_3, 2); - add_motor(AP_MOTORS_MOT_5, -60, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_6, 6); - add_motor(AP_MOTORS_MOT_6, 120, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_5, 3); + add_motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_MOTOR_CW, 1); + add_motor(AP_MOTORS_MOT_2, 180, AP_MOTORS_MATRIX_MOTOR_CCW, 4); + add_motor(AP_MOTORS_MOT_3,-120, AP_MOTORS_MATRIX_MOTOR_CW, 5); + add_motor(AP_MOTORS_MOT_4, 60, AP_MOTORS_MATRIX_MOTOR_CCW, 2); + add_motor(AP_MOTORS_MOT_5, -60, AP_MOTORS_MATRIX_MOTOR_CCW, 6); + add_motor(AP_MOTORS_MOT_6, 120, AP_MOTORS_MATRIX_MOTOR_CW, 3); }else{ // X frame set-up - add_motor(AP_MOTORS_MOT_1, 90, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_2, 2); - add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_1, 5); - add_motor(AP_MOTORS_MOT_3, -30, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_4, 6); - add_motor(AP_MOTORS_MOT_4, 150, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_3, 3); - add_motor(AP_MOTORS_MOT_5, 30, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_6, 1); - add_motor(AP_MOTORS_MOT_6,-150, AP_MOTORS_MATRIX_MOTOR_CW, AP_MOTORS_MOT_5, 4); + add_motor(AP_MOTORS_MOT_1, 90, AP_MOTORS_MATRIX_MOTOR_CW, 2); + add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_MOTOR_CCW, 5); + add_motor(AP_MOTORS_MOT_3, -30, AP_MOTORS_MATRIX_MOTOR_CW, 6); + add_motor(AP_MOTORS_MOT_4, 150, AP_MOTORS_MATRIX_MOTOR_CCW, 3); + add_motor(AP_MOTORS_MOT_5, 30, AP_MOTORS_MATRIX_MOTOR_CCW, 1); + add_motor(AP_MOTORS_MOT_6,-150, AP_MOTORS_MATRIX_MOTOR_CW, 4); } } \ No newline at end of file diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index 8e0c3e783a..726a8d11d0 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -15,20 +15,12 @@ extern const AP_HAL::HAL& hal; // Init void AP_MotorsMatrix::Init() { - int8_t i; - // call parent Init function to set-up throttle curve AP_Motors::Init(); // setup the motors setup_motors(); - // double check that opposite motor definitions are ok - for( i=0; i= AP_MOTORS_MAX_NUM_MOTORS || !motor_enabled[opposite_motor[i]] ) - opposite_motor[i] = AP_MOTORS_MATRIX_MOTOR_UNDEFINED; - } - // enable fast channels or instant pwm set_update_rate(_speed_hz); } @@ -328,7 +320,7 @@ void AP_MotorsMatrix::output_test() } // add_motor -void AP_MotorsMatrix::add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, int8_t opposite_motor_num, int8_t testing_order) +void AP_MotorsMatrix::add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, int8_t testing_order) { // ensure valid motor number is provided if( motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS ) { @@ -344,13 +336,6 @@ void AP_MotorsMatrix::add_motor_raw(int8_t motor_num, float roll_fac, float pitc _pitch_factor[motor_num] = pitch_fac; _yaw_factor[motor_num] = yaw_fac; - // set opposite motor after checking it's somewhat valid - if( opposite_motor_num == AP_MOTORS_MATRIX_MOTOR_UNDEFINED || (opposite_motor_num >=0 && opposite_motor_num < AP_MOTORS_MAX_NUM_MOTORS) ) { - opposite_motor[motor_num] = opposite_motor_num; - }else{ - opposite_motor[motor_num] = AP_MOTORS_MATRIX_MOTOR_UNDEFINED; - } - // set order that motor appears in test if( testing_order == AP_MOTORS_MATRIX_ORDER_UNDEFINED ) { test_order[motor_num] = motor_num; @@ -361,7 +346,7 @@ void AP_MotorsMatrix::add_motor_raw(int8_t motor_num, float roll_fac, float pitc } // add_motor using just position and prop direction -void AP_MotorsMatrix::add_motor(int8_t motor_num, float angle_degrees, int8_t direction, int8_t opposite_motor_num, int8_t testing_order) +void AP_MotorsMatrix::add_motor(int8_t motor_num, float angle_degrees, int8_t direction, int8_t testing_order) { // call raw motor set-up method add_motor_raw( @@ -369,7 +354,6 @@ void AP_MotorsMatrix::add_motor(int8_t motor_num, float angle_degrees, int8_t di cos(radians(angle_degrees + 90)), // roll factor cos(radians(angle_degrees)), // pitch factor (float)direction, // yaw factor - opposite_motor_num, testing_order); } @@ -377,8 +361,6 @@ void AP_MotorsMatrix::add_motor(int8_t motor_num, float angle_degrees, int8_t di // remove_motor - disabled motor and clears all roll, pitch, throttle factors for this motor void AP_MotorsMatrix::remove_motor(int8_t motor_num) { - int8_t i; - // ensure valid motor number is provided if( motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS ) { @@ -391,13 +373,6 @@ void AP_MotorsMatrix::remove_motor(int8_t motor_num) _roll_factor[motor_num] = 0; _pitch_factor[motor_num] = 0; _yaw_factor[motor_num] = 0; - opposite_motor[motor_num] = AP_MOTORS_MATRIX_MOTOR_UNDEFINED; - } - - // if another motor has referred to this motor as it's opposite, remove that reference - for( i=0; i