Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add Y4 Frame type #19056

Merged
merged 2 commits into from
Nov 4, 2021
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion ArduCopter/Parameters.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -342,7 +342,7 @@ const AP_Param::Info Copter::var_info[] = {
// @Param: FRAME_TYPE
// @DisplayName: Frame Type (+, X, V, etc)
// @Description: Controls motor mixing for multicopters. Not used for Tri or Traditional Helicopters.
// @Values: 0:Plus, 1:X, 2:V, 3:H, 4:V-Tail, 5:A-Tail, 10:Y6B, 11:Y6F, 12:BetaFlightX, 13:DJIX, 14:ClockwiseX, 15: I, 18: BetaFlightXReversed
// @Values: 0:Plus, 1:X, 2:V, 3:H, 4:V-Tail, 5:A-Tail, 10:Y6B, 11:Y6F, 12:BetaFlightX, 13:DJIX, 14:ClockwiseX, 15: I, 18: BetaFlightXReversed, 19:Y4
// @User: Standard
// @RebootRequired: True
GSCALAR(frame_type, "FRAME_TYPE", HAL_FRAME_TYPE_DEFAULT),
Expand Down
11 changes: 11 additions & 0 deletions libraries/AP_Motors/AP_MotorsMatrix.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -765,6 +765,17 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
add_motors(motors, ARRAY_SIZE(motors));
break;
}
case MOTOR_FRAME_TYPE_Y4:
_frame_type_string = "Y4";
// Y4 motor definition with right front CCW, left front CW
static const AP_MotorsMatrix::MotorDefRaw motors[] {
{ -1.0f, 1.000f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1 },
{ 0.0f, -1.000f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2 },
{ 0.0f, -1.000f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3 },
{ 1.0f, 1.000f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4 },
};
add_motors_raw(motors, ARRAY_SIZE(motors));
break;
default:
// quad frame class does not support this frame type
_frame_type_string = "UNSUPPORTED";
Expand Down
1 change: 1 addition & 0 deletions libraries/AP_Motors/AP_Motors_Class.h
Original file line number Diff line number Diff line change
Expand Up @@ -92,6 +92,7 @@ class AP_Motors {
MOTOR_FRAME_TYPE_NYT_PLUS = 16, // plus frame, no differential torque for yaw
MOTOR_FRAME_TYPE_NYT_X = 17, // X frame, no differential torque for yaw
MOTOR_FRAME_TYPE_BF_X_REV = 18, // X frame, betaflight ordering, reversed motors
MOTOR_FRAME_TYPE_Y4 = 19, //Y4 Quadrotor frame
};

// return string corresponding to frame_type
Expand Down