From cb5f8826f89fd3c4ebb73899eca4642ca452f776 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 25 Apr 2013 17:52:19 +0900 Subject: [PATCH] Copter: Leonard's fix for trapezoidal frame We need to allow yaw_factors to be floats in the add_motor method --- libraries/AP_Motors/AP_MotorsHexa.cpp | 24 ++++++------ libraries/AP_Motors/AP_MotorsMatrix.cpp | 4 +- libraries/AP_Motors/AP_MotorsMatrix.h | 8 ++-- 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, 72 insertions(+), 72 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHexa.cpp b/libraries/AP_Motors/AP_MotorsHexa.cpp index 46d84b1c57..6282a7901f 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, 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); + add_motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1); + add_motor(AP_MOTORS_MOT_2, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4); + add_motor(AP_MOTORS_MOT_3,-120, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5); + add_motor(AP_MOTORS_MOT_4, 60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2); + add_motor(AP_MOTORS_MOT_5, -60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6); + add_motor(AP_MOTORS_MOT_6, 120, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3); }else{ // X frame set-up - 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); + add_motor(AP_MOTORS_MOT_1, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2); + add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5); + add_motor(AP_MOTORS_MOT_3, -30, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6); + add_motor(AP_MOTORS_MOT_4, 150, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3); + add_motor(AP_MOTORS_MOT_5, 30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1); + add_motor(AP_MOTORS_MOT_6,-150, AP_MOTORS_MATRIX_YAW_FACTOR_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 69b45ae826..96534cd892 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -342,14 +342,14 @@ 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 testing_order) +void AP_MotorsMatrix::add_motor(int8_t motor_num, float angle_degrees, float yaw_factor, int8_t testing_order) { // call raw motor set-up method add_motor_raw( motor_num, cosf(radians(angle_degrees + 90)), // roll factor cosf(radians(angle_degrees)), // pitch factor - (float)direction, // yaw factor + yaw_factor, // yaw factor testing_order); } diff --git a/libraries/AP_Motors/AP_MotorsMatrix.h b/libraries/AP_Motors/AP_MotorsMatrix.h index f384ef2f6e..6499e60c46 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.h +++ b/libraries/AP_Motors/AP_MotorsMatrix.h @@ -14,8 +14,8 @@ #define AP_MOTORS_MATRIX_MOTOR_UNDEFINED -1 #define AP_MOTORS_MATRIX_ORDER_UNDEFINED -1 -#define AP_MOTORS_MATRIX_MOTOR_CW -1 -#define AP_MOTORS_MATRIX_MOTOR_CCW 1 +#define AP_MOTORS_MATRIX_YAW_FACTOR_CW -1 +#define AP_MOTORS_MATRIX_YAW_FACTOR_CCW 1 #define AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM 100 @@ -53,8 +53,8 @@ public: // output_min - sends minimum values out to the motors virtual void output_min(); - // add_motor using just position and prop direction - virtual void add_motor(int8_t motor_num, float angle_degrees, int8_t direction, int8_t testing_order = AP_MOTORS_MATRIX_ORDER_UNDEFINED); + // add_motor using just position and yaw_factor (or prop direction) + virtual void add_motor(int8_t motor_num, float angle_degrees, float yaw_factor, int8_t testing_order = AP_MOTORS_MATRIX_ORDER_UNDEFINED); // remove_motor virtual void remove_motor(int8_t motor_num); diff --git a/libraries/AP_Motors/AP_MotorsOcta.cpp b/libraries/AP_Motors/AP_MotorsOcta.cpp index adb3c0cbfd..58af193123 100644 --- a/libraries/AP_Motors/AP_MotorsOcta.cpp +++ b/libraries/AP_Motors/AP_MotorsOcta.cpp @@ -19,35 +19,35 @@ void AP_MotorsOcta::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, 1); - add_motor(AP_MOTORS_MOT_2, 180, AP_MOTORS_MATRIX_MOTOR_CW, 5); - add_motor(AP_MOTORS_MOT_3, 45, AP_MOTORS_MATRIX_MOTOR_CCW, 2); - add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_MOTOR_CCW, 4); - add_motor(AP_MOTORS_MOT_5, -45, AP_MOTORS_MATRIX_MOTOR_CCW, 8); - add_motor(AP_MOTORS_MOT_6, -135, AP_MOTORS_MATRIX_MOTOR_CCW, 6); - add_motor(AP_MOTORS_MOT_7, -90, AP_MOTORS_MATRIX_MOTOR_CW, 7); - add_motor(AP_MOTORS_MOT_8, 90, AP_MOTORS_MATRIX_MOTOR_CW, 3); + add_motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1); + add_motor(AP_MOTORS_MOT_2, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5); + add_motor(AP_MOTORS_MOT_3, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2); + add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4); + add_motor(AP_MOTORS_MOT_5, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8); + add_motor(AP_MOTORS_MOT_6, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6); + add_motor(AP_MOTORS_MOT_7, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7); + add_motor(AP_MOTORS_MOT_8, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3); }else if( _frame_orientation == AP_MOTORS_V_FRAME ) { // V frame set-up - add_motor_raw(AP_MOTORS_MOT_1, 1.0, 0.34, AP_MOTORS_MATRIX_MOTOR_CW, 7); - add_motor_raw(AP_MOTORS_MOT_2, -1.0, -0.32, AP_MOTORS_MATRIX_MOTOR_CW, 3); - add_motor_raw(AP_MOTORS_MOT_3, 1.0, -0.32, AP_MOTORS_MATRIX_MOTOR_CCW, 6); - add_motor_raw(AP_MOTORS_MOT_4, -0.5, -1.0, AP_MOTORS_MATRIX_MOTOR_CCW, 4); - add_motor_raw(AP_MOTORS_MOT_5, 1.0, 1.0, AP_MOTORS_MATRIX_MOTOR_CCW, 8); - add_motor_raw(AP_MOTORS_MOT_6, -1.0, 0.34, AP_MOTORS_MATRIX_MOTOR_CCW, 2); - add_motor_raw(AP_MOTORS_MOT_7, -1.0, 1.0, AP_MOTORS_MATRIX_MOTOR_CW, 1); - add_motor_raw(AP_MOTORS_MOT_8, 0.5, -1.0, AP_MOTORS_MATRIX_MOTOR_CW, 5); + add_motor_raw(AP_MOTORS_MOT_1, 1.0, 0.34, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7); + add_motor_raw(AP_MOTORS_MOT_2, -1.0, -0.32, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3); + add_motor_raw(AP_MOTORS_MOT_3, 1.0, -0.32, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6); + add_motor_raw(AP_MOTORS_MOT_4, -0.5, -1.0, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4); + add_motor_raw(AP_MOTORS_MOT_5, 1.0, 1.0, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8); + add_motor_raw(AP_MOTORS_MOT_6, -1.0, 0.34, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2); + add_motor_raw(AP_MOTORS_MOT_7, -1.0, 1.0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1); + add_motor_raw(AP_MOTORS_MOT_8, 0.5, -1.0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5); }else { // X frame set-up - add_motor(AP_MOTORS_MOT_1, 22.5, AP_MOTORS_MATRIX_MOTOR_CW, 1); - add_motor(AP_MOTORS_MOT_2, -157.5, AP_MOTORS_MATRIX_MOTOR_CW, 5); - add_motor(AP_MOTORS_MOT_3, 67.5, AP_MOTORS_MATRIX_MOTOR_CCW, 2); - add_motor(AP_MOTORS_MOT_4, 157.5, AP_MOTORS_MATRIX_MOTOR_CCW, 4); - add_motor(AP_MOTORS_MOT_5, -22.5, AP_MOTORS_MATRIX_MOTOR_CCW, 8); - add_motor(AP_MOTORS_MOT_6, -112.5, AP_MOTORS_MATRIX_MOTOR_CCW, 6); - add_motor(AP_MOTORS_MOT_7, -67.5, AP_MOTORS_MATRIX_MOTOR_CW, 7); - add_motor(AP_MOTORS_MOT_8, 112.5, AP_MOTORS_MATRIX_MOTOR_CW, 3); + add_motor(AP_MOTORS_MOT_1, 22.5, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1); + add_motor(AP_MOTORS_MOT_2, -157.5, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5); + add_motor(AP_MOTORS_MOT_3, 67.5, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2); + add_motor(AP_MOTORS_MOT_4, 157.5, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4); + add_motor(AP_MOTORS_MOT_5, -22.5, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8); + add_motor(AP_MOTORS_MOT_6, -112.5, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6); + add_motor(AP_MOTORS_MOT_7, -67.5, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7); + add_motor(AP_MOTORS_MOT_8, 112.5, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3); } } \ No newline at end of file diff --git a/libraries/AP_Motors/AP_MotorsOctaQuad.cpp b/libraries/AP_Motors/AP_MotorsOctaQuad.cpp index ce6e66c87a..4afa465b2f 100644 --- a/libraries/AP_Motors/AP_MotorsOctaQuad.cpp +++ b/libraries/AP_Motors/AP_MotorsOctaQuad.cpp @@ -19,23 +19,23 @@ void AP_MotorsOctaQuad::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_CCW, 1); - add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_MOTOR_CW, 7); - add_motor(AP_MOTORS_MOT_3, 180, AP_MOTORS_MATRIX_MOTOR_CCW, 5); - add_motor(AP_MOTORS_MOT_4, 90, AP_MOTORS_MATRIX_MOTOR_CW, 3); - add_motor(AP_MOTORS_MOT_5, -90, AP_MOTORS_MATRIX_MOTOR_CCW, 8); - add_motor(AP_MOTORS_MOT_6, 0, AP_MOTORS_MATRIX_MOTOR_CW, 2); - add_motor(AP_MOTORS_MOT_7, 90, AP_MOTORS_MATRIX_MOTOR_CCW, 4); - add_motor(AP_MOTORS_MOT_8, 180, AP_MOTORS_MATRIX_MOTOR_CW, 6); + add_motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1); + add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7); + add_motor(AP_MOTORS_MOT_3, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5); + add_motor(AP_MOTORS_MOT_4, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3); + add_motor(AP_MOTORS_MOT_5, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8); + add_motor(AP_MOTORS_MOT_6, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2); + add_motor(AP_MOTORS_MOT_7, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4); + add_motor(AP_MOTORS_MOT_8, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6); }else{ // X frame set-up - add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_MOTOR_CCW, 1); - add_motor(AP_MOTORS_MOT_2, -45, AP_MOTORS_MATRIX_MOTOR_CW, 7); - add_motor(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_MOTOR_CCW, 5); - add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_MOTOR_CW, 3); - add_motor(AP_MOTORS_MOT_5, -45, AP_MOTORS_MATRIX_MOTOR_CCW, 8); - add_motor(AP_MOTORS_MOT_6, 45, AP_MOTORS_MATRIX_MOTOR_CW, 2); - add_motor(AP_MOTORS_MOT_7, 135, AP_MOTORS_MATRIX_MOTOR_CCW, 4); - add_motor(AP_MOTORS_MOT_8, -135, AP_MOTORS_MATRIX_MOTOR_CW, 6); + add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1); + add_motor(AP_MOTORS_MOT_2, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7); + add_motor(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5); + add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3); + add_motor(AP_MOTORS_MOT_5, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8); + add_motor(AP_MOTORS_MOT_6, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2); + add_motor(AP_MOTORS_MOT_7, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4); + add_motor(AP_MOTORS_MOT_8, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6); } } diff --git a/libraries/AP_Motors/AP_MotorsQuad.cpp b/libraries/AP_Motors/AP_MotorsQuad.cpp index ec05f2cc89..af42bba718 100644 --- a/libraries/AP_Motors/AP_MotorsQuad.cpp +++ b/libraries/AP_Motors/AP_MotorsQuad.cpp @@ -19,10 +19,10 @@ void AP_MotorsQuad::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, 90, AP_MOTORS_MATRIX_MOTOR_CCW, 2); - add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_MOTOR_CCW, 4); - add_motor(AP_MOTORS_MOT_3, 0, AP_MOTORS_MATRIX_MOTOR_CW, 1); - add_motor(AP_MOTORS_MOT_4, 180, AP_MOTORS_MATRIX_MOTOR_CW, 3); + add_motor(AP_MOTORS_MOT_1, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2); + add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4); + add_motor(AP_MOTORS_MOT_3, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1); + add_motor(AP_MOTORS_MOT_4, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3); }else if( _frame_orientation == AP_MOTORS_V_FRAME ) { add_motor(AP_MOTORS_MOT_1, 45, 0.7981, 1); add_motor(AP_MOTORS_MOT_2, -135, 1.0000, 3); @@ -30,9 +30,9 @@ void AP_MotorsQuad::setup_motors() add_motor(AP_MOTORS_MOT_4, 135, -1.0000, 2); }else{ // X frame set-up - add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_MOTOR_CCW, 1); - add_motor(AP_MOTORS_MOT_2, -135, AP_MOTORS_MATRIX_MOTOR_CCW, 3); - add_motor(AP_MOTORS_MOT_3, -45, AP_MOTORS_MATRIX_MOTOR_CW, 4); - add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_MOTOR_CW, 2); + add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1); + add_motor(AP_MOTORS_MOT_2, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3); + add_motor(AP_MOTORS_MOT_3, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4); + add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2); } } diff --git a/libraries/AP_Motors/AP_MotorsY6.cpp b/libraries/AP_Motors/AP_MotorsY6.cpp index 3e00ff047e..a2550c0009 100644 --- a/libraries/AP_Motors/AP_MotorsY6.cpp +++ b/libraries/AP_Motors/AP_MotorsY6.cpp @@ -17,10 +17,10 @@ void AP_MotorsY6::setup_motors() AP_MotorsMatrix::setup_motors(); // MultiWii set-up - add_motor_raw(AP_MOTORS_MOT_1, -1.0, 0.666, AP_MOTORS_MATRIX_MOTOR_CCW, 2); - add_motor_raw(AP_MOTORS_MOT_2, 1.0, 0.666, AP_MOTORS_MATRIX_MOTOR_CW, 5); - add_motor_raw(AP_MOTORS_MOT_3, 1.0, 0.666, AP_MOTORS_MATRIX_MOTOR_CCW, 6); - add_motor_raw(AP_MOTORS_MOT_4, 0.0, -1.333, AP_MOTORS_MATRIX_MOTOR_CW, 4); - add_motor_raw(AP_MOTORS_MOT_5, -1.0, 0.666, AP_MOTORS_MATRIX_MOTOR_CW, 1); - add_motor_raw(AP_MOTORS_MOT_6, 0.0, -1.333, AP_MOTORS_MATRIX_MOTOR_CCW, 3); + add_motor_raw(AP_MOTORS_MOT_1, -1.0, 0.666, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2); + add_motor_raw(AP_MOTORS_MOT_2, 1.0, 0.666, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5); + add_motor_raw(AP_MOTORS_MOT_3, 1.0, 0.666, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6); + add_motor_raw(AP_MOTORS_MOT_4, 0.0, -1.333, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4); + add_motor_raw(AP_MOTORS_MOT_5, -1.0, 0.666, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1); + add_motor_raw(AP_MOTORS_MOT_6, 0.0, -1.333, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3); } \ No newline at end of file