From 1cfd5900a861e947ccd5fd207dfeb6a9b39821f2 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 3 May 2013 23:13:40 +0900 Subject: [PATCH] Copter: add support for H-quad frame H-quads are like X quads but the motors spin in the opposite direction. Having the motors reversed seems to help with yaw authority because of the slight flex in bodies --- ArduCopter/Parameters.pde | 2 +- libraries/AP_Motors/AP_MotorsQuad.cpp | 7 +++++++ libraries/AP_Motors/AP_Motors_Class.h | 1 + 3 files changed, 9 insertions(+), 1 deletion(-) diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index d15f264688..9198a9449d 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -394,7 +394,7 @@ const AP_Param::Info var_info[] PROGMEM = { // @Param: FRAME // @DisplayName: Frame Orientation (+, X or V) // @Description: Controls motor mixing for multicopters. Not used for Tri or Traditional Helicopters. - // @Values: 0:Plus, 1:X, 2:V + // @Values: 0:Plus, 1:X, 2:V, 3:H // @User: Standard // @Range: 0 32767 GSCALAR(frame_orientation, "FRAME", FRAME_ORIENTATION), diff --git a/libraries/AP_Motors/AP_MotorsQuad.cpp b/libraries/AP_Motors/AP_MotorsQuad.cpp index af42bba718..6c8f52eed1 100644 --- a/libraries/AP_Motors/AP_MotorsQuad.cpp +++ b/libraries/AP_Motors/AP_MotorsQuad.cpp @@ -24,10 +24,17 @@ void AP_MotorsQuad::setup_motors() 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 ) { + // V frame set-up add_motor(AP_MOTORS_MOT_1, 45, 0.7981, 1); add_motor(AP_MOTORS_MOT_2, -135, 1.0000, 3); add_motor(AP_MOTORS_MOT_3, -45, -0.7981, 4); add_motor(AP_MOTORS_MOT_4, 135, -1.0000, 2); + }else if( _frame_orientation == AP_MOTORS_H_FRAME ) { + // H frame set-up - same as X but motors spin in opposite directions + add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1); + add_motor(AP_MOTORS_MOT_2, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3); + add_motor(AP_MOTORS_MOT_3, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4); + add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2); }else{ // X frame set-up add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1); diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 4027276026..350bce8dd6 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -35,6 +35,7 @@ #define AP_MOTORS_PLUS_FRAME 0 #define AP_MOTORS_X_FRAME 1 #define AP_MOTORS_V_FRAME 2 +#define AP_MOTORS_H_FRAME 3 // same as X frame but motors spin in opposite direction // motor update rate #define AP_MOTORS_SPEED_DEFAULT 490