From b187a0c6eba7b9bd32050ca52ca313ca833634c6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 24 Nov 2015 10:18:09 +1100 Subject: [PATCH] AP_Motors: added quadplane frame type this puts the motors on outputs 5 to 8, to leave the first 4 for the plane --- libraries/AP_Motors/AP_MotorsQuad.cpp | 6 ++++++ libraries/AP_Motors/AP_Motors_Class.h | 1 + 2 files changed, 7 insertions(+) diff --git a/libraries/AP_Motors/AP_MotorsQuad.cpp b/libraries/AP_Motors/AP_MotorsQuad.cpp index cd92f2997a..3b0e8ab0e2 100644 --- a/libraries/AP_Motors/AP_MotorsQuad.cpp +++ b/libraries/AP_Motors/AP_MotorsQuad.cpp @@ -88,6 +88,12 @@ void AP_MotorsQuad::setup_motors() add_motor(AP_MOTORS_MOT_2, 0, -160, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3); add_motor(AP_MOTORS_MOT_3, -60, -60, 0, 4); add_motor(AP_MOTORS_MOT_4, 0, 160, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2); + } else if ( _flags.frame_orientation == AP_MOTORS_QUADPLANE ) { + // quadplane frame set-up, X arrangement on motors 5 to 8 + add_motor(AP_MOTORS_MOT_5, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1); + add_motor(AP_MOTORS_MOT_6, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3); + add_motor(AP_MOTORS_MOT_7, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4); + add_motor(AP_MOTORS_MOT_8, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 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 6b2664e270..060b3e56db 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -32,6 +32,7 @@ #define AP_MOTORS_NEW_X_FRAME 11 #define AP_MOTORS_NEW_V_FRAME 12 #define AP_MOTORS_NEW_H_FRAME 13 // same as X frame but motors spin in opposite direction +#define AP_MOTORS_QUADPLANE 14 // motors on 5..8 // motor update rate #define AP_MOTORS_SPEED_DEFAULT 490 // default output rate to the motors