From 8149b548077fba27f0d552f188bf7a2612013ac0 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 3 Dec 2013 23:22:14 +0900 Subject: [PATCH] Copter: add Y6 with all top props CW Set FRAME parameter to 10 --- libraries/AP_Motors/AP_MotorsY6.cpp | 24 +++++++++++++++++------- libraries/AP_Motors/AP_Motors_Class.h | 6 +++++- 2 files changed, 22 insertions(+), 8 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsY6.cpp b/libraries/AP_Motors/AP_MotorsY6.cpp index aa70dde73b..cabd965389 100644 --- a/libraries/AP_Motors/AP_MotorsY6.cpp +++ b/libraries/AP_Motors/AP_MotorsY6.cpp @@ -28,11 +28,21 @@ void AP_MotorsY6::setup_motors() // call parent AP_MotorsMatrix::setup_motors(); - // MultiWii set-up - 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); + if (_flags.frame_orientation >= AP_MOTORS_NEW_PLUS_FRAME) { + // Y6 motor definition with all top motors spinning clockwise, all bottom motors counter clockwise + add_motor_raw(AP_MOTORS_MOT_1, -1.0, 0.666, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1); + add_motor_raw(AP_MOTORS_MOT_2, -1.0, 0.666, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2); + add_motor_raw(AP_MOTORS_MOT_3, 0.0, -1.333, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3); + add_motor_raw(AP_MOTORS_MOT_4, 0.0, -1.333, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4); + add_motor_raw(AP_MOTORS_MOT_5, 1.0, 0.666, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5); + add_motor_raw(AP_MOTORS_MOT_6, 1.0, 0.666, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6); + }else{ + // original Y6 motor definition + 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); + } } diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 3032e7d17d..037907b4a2 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -38,6 +38,10 @@ #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 +#define AP_MOTORS_NEW_PLUS_FRAME 10 // NEW frames are same as original 4 but with motor orders changed to be clockwise from the front +#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 // motor update rate #define AP_MOTORS_SPEED_DEFAULT 490 // default output rate to the motors @@ -146,7 +150,7 @@ protected: // flag bitmask struct AP_Motors_flags { uint8_t armed : 1; // 1 if the motors are armed, 0 if disarmed - uint8_t frame_orientation : 2; // PLUS_FRAME 0, X_FRAME 1, V_FRAME 2, H_FRAME 3 + uint8_t frame_orientation : 4; // PLUS_FRAME 0, X_FRAME 1, V_FRAME 2, H_FRAME 3, NEW_PLUS_FRAME 10, NEW_X_FRAME, NEW_V_FRAME, NEW_H_FRAME uint8_t slow_start : 1; // 1 if slow start is active uint8_t slow_start_low_end : 1; // 1 just after arming so we can ramp up the spin_when_armed value } _flags;