diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index c1fb8d3423..ba8bf3eced 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -182,9 +182,6 @@ protected: // calculate_scalars - must be implemented by child classes virtual void calculate_scalars() = 0; - // calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position - virtual void calculate_roll_pitch_collective_factors() = 0; - // servo_test - move servos through full range of movement // to be overloaded by child classes, different vehicle types would have different movement patterns virtual void servo_test() = 0; diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp index 631500c84a..4b11fdb346 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp @@ -23,77 +23,9 @@ extern const AP_HAL::HAL& hal; const AP_Param::GroupInfo AP_MotorsHeli_Dual::var_info[] = { AP_NESTEDGROUPINFO(AP_MotorsHeli, 0), - // @Param: SV1_POS - // @DisplayName: Servo 1 Position - // @Description: Angular location of swash servo #1 - // @Range: -180 180 - // @Units: deg - // @User: Standard - // @Increment: 1 - AP_GROUPINFO("SV1_POS", 1, AP_MotorsHeli_Dual, _servo1_pos, AP_MOTORS_HELI_DUAL_SERVO1_POS), - - // @Param: SV2_POS - // @DisplayName: Servo 2 Position - // @Description: Angular location of swash servo #2 - // @Range: -180 180 - // @Units: deg - // @User: Standard - // @Increment: 1 - AP_GROUPINFO("SV2_POS", 2, AP_MotorsHeli_Dual, _servo2_pos, AP_MOTORS_HELI_DUAL_SERVO2_POS), + // Indices 1-6 were used by servo position params and should not be used - // @Param: SV3_POS - // @DisplayName: Servo 3 Position - // @Description: Angular location of swash servo #3 - // @Range: -180 180 - // @Units: deg - // @User: Standard - // @Increment: 1 - AP_GROUPINFO("SV3_POS", 3, AP_MotorsHeli_Dual, _servo3_pos, AP_MOTORS_HELI_DUAL_SERVO3_POS), - - // @Param: SV4_POS - // @DisplayName: Servo 4 Position - // @Description: Angular location of swash servo #4 - // @Range: -180 180 - // @Units: deg - // @User: Standard - // @Increment: 1 - AP_GROUPINFO("SV4_POS", 4, AP_MotorsHeli_Dual, _servo4_pos, AP_MOTORS_HELI_DUAL_SERVO4_POS), - - // @Param: SV5_POS - // @DisplayName: Servo 5 Position - // @Description: Angular location of swash servo #5 - // @Range: -180 180 - // @Units: deg - // @User: Standard - // @Increment: 1 - AP_GROUPINFO("SV5_POS", 5, AP_MotorsHeli_Dual, _servo5_pos, AP_MOTORS_HELI_DUAL_SERVO5_POS), - - // @Param: SV6_POS - // @DisplayName: Servo 6 Position - // @Description: Angular location of swash servo #6 - // @Range: -180 180 - // @Units: deg - // @User: Standard - // @Increment: 1 - AP_GROUPINFO("SV6_POS", 6, AP_MotorsHeli_Dual, _servo6_pos, AP_MOTORS_HELI_DUAL_SERVO6_POS), - - // @Param: PHANG1 - // @DisplayName: Swashplate 1 Phase Angle Compensation - // @Description: Phase angle correction for rotor head. If pitching the swash forward induces a roll, this can be correct the problem - // @Range: -90 90 - // @Units: deg - // @User: Advanced - // @Increment: 1 - AP_GROUPINFO("PHANG1", 7, AP_MotorsHeli_Dual, _swash1_phase_angle, 0), - - // @Param: PHANG2 - // @DisplayName: Swashplate 2 Phase Angle Compensation - // @Description: Phase angle correction for rotor head. If pitching the swash forward induces a roll, this can be correct the problem - // @Range: -90 90 - // @Units: deg - // @User: Advanced - // @Increment: 1 - AP_GROUPINFO("PHANG2", 8, AP_MotorsHeli_Dual, _swash2_phase_angle, 0), + // Indices 7-8 were used by phase angle params and should not be used // @Param: DUAL_MODE // @DisplayName: Dual Mode @@ -153,11 +85,41 @@ const AP_Param::GroupInfo AP_MotorsHeli_Dual::var_info[] = { AP_GROUPINFO("COL2_MID", 18, AP_MotorsHeli_Dual, _collective2_mid, AP_MOTORS_HELI_DUAL_COLLECTIVE2_MID), // @Param: COL_CTRL_DIR - // @DisplayName: Collective Control Direction + // @DisplayName: Collective Control Direction for swash 1 + // @Description: Direction collective moves for positive pitch. 0 for Normal, 1 for Reversed + // @Values: 0:Normal,1:Reversed + // @User: Standard + AP_GROUPINFO("COL_CTRL_DIR", 19, AP_MotorsHeli_Dual, _swash1_coll_dir, (int8_t)COLLECTIVE_DIRECTION_NORMAL), + + // @Param: COL_CTRL_DIR2 + // @DisplayName: Collective Control Direction for swash 2 // @Description: Direction collective moves for positive pitch. 0 for Normal, 1 for Reversed // @Values: 0:Normal,1:Reversed // @User: Standard - AP_GROUPINFO("COL_CTRL_DIR", 19, AP_MotorsHeli_Dual, _collective_direction, AP_MOTORS_HELI_DUAL_COLLECTIVE_DIRECTION_NORMAL), + AP_GROUPINFO("COL_CTRL_DIR2", 20, AP_MotorsHeli_Dual, _swash2_coll_dir, (int8_t)COLLECTIVE_DIRECTION_NORMAL), + + // @Param: SWASH1_TYPE + // @DisplayName: Swash Type for swashplate 1 + // @Description: Swash Type Setting + // @Values: 0:H3 CCPM Adjustable, 1:H1 Straight Swash, 2:H3_140 CCPM + // @User: Standard + AP_GROUPINFO("SWASH1_TYPE", 21, AP_MotorsHeli_Dual, _swashplate1_type, (int8_t)SWASHPLATE_TYPE_H3), + + // @Param: SWASH2_TYPE + // @DisplayName: Swash Type for swashplate 2 + // @Description: Swash Type Setting + // @Values: 0:H3 CCPM Adjustable, 1:H1 Straight Swash, 2:H3_140 CCPM + // @User: Standard + AP_GROUPINFO("SWASH2_TYPE", 22, AP_MotorsHeli_Dual, _swashplate2_type, (int8_t)SWASHPLATE_TYPE_H3), + + // @Group: SW1_H3_ + // @Path: Swash.cpp + AP_SUBGROUPINFO(_swash1_H3, "SW1_H3_", 23, AP_MotorsHeli_Dual, SwashInt16Param), + + // @Group: SW2_H3_ + // @Path: Swash.cpp + AP_SUBGROUPINFO(_swash2_H3, "SW2_H3_", 24, AP_MotorsHeli_Dual, SwashInt16Param), + AP_GROUPEND }; @@ -173,6 +135,12 @@ void AP_MotorsHeli_Dual::set_update_rate( uint16_t speed_hz ) for (uint8_t i=0; i((uint8_t)_swashplate1_type)); + _swashplate1.set_collective_direction(static_cast((uint8_t)_swash1_coll_dir)); + _swashplate1.calculate_roll_pitch_collective_factors(); + + // configure swashplate 2 and update scalars + if (_swashplate2_type == SWASHPLATE_TYPE_H3) { + if (_swash2_H3.get_enable() == 0) { + _swash2_H3.set_enable(1); + } + _swashplate2.set_servo1_pos(_swash2_H3.get_servo1_pos()); + _swashplate2.set_servo2_pos(_swash2_H3.get_servo2_pos()); + _swashplate2.set_servo3_pos(_swash2_H3.get_servo3_pos()); + _swashplate2.set_phase_angle(_swash2_H3.get_phase_angle()); + } else { + if (_swash2_H3.get_enable() == 1) { + _swash2_H3.set_enable(0); + } + } + _swashplate2.set_swash_type(static_cast((uint8_t)_swashplate2_type)); + _swashplate2.set_collective_direction(static_cast((uint8_t)_swash2_coll_dir)); + _swashplate2.calculate_roll_pitch_collective_factors(); // set mode of main rotor controller and trigger recalculation of scalars _rotor.set_control_mode(static_cast(_rsc_mode.get())); calculate_armed_scalars(); } -// calculate_swash_factors - calculate factors based on swash type and servo position -// To Do: support H3-140 swashplates in Heli Dual? -void AP_MotorsHeli_Dual::calculate_roll_pitch_collective_factors() +// get_swashplate - calculate movement of each swashplate based on configuration +float AP_MotorsHeli_Dual::get_swashplate (int8_t swash_num, int8_t swash_axis, float pitch_input, float roll_input, float yaw_input, float coll_input) { + float swash_tilt = 0.0f; if (_dual_mode == AP_MOTORS_HELI_DUAL_MODE_TRANSVERSE) { - // roll factors - _rollFactor[CH_1] = _dcp_scaler; - _rollFactor[CH_2] = _dcp_scaler; - _rollFactor[CH_3] = _dcp_scaler; - - _rollFactor[CH_4] = -_dcp_scaler; - _rollFactor[CH_5] = -_dcp_scaler; - _rollFactor[CH_6] = -_dcp_scaler; - - // pitch factors - _pitchFactor[CH_1] = cosf(radians(_servo1_pos - _swash1_phase_angle)); - _pitchFactor[CH_2] = cosf(radians(_servo2_pos - _swash1_phase_angle)); - _pitchFactor[CH_3] = cosf(radians(_servo3_pos - _swash1_phase_angle)); - - _pitchFactor[CH_4] = cosf(radians(_servo4_pos - _swash2_phase_angle)); - _pitchFactor[CH_5] = cosf(radians(_servo5_pos - _swash2_phase_angle)); - _pitchFactor[CH_6] = cosf(radians(_servo6_pos - _swash2_phase_angle)); - - // yaw factors - _yawFactor[CH_1] = cosf(radians(_servo1_pos + 180 - _swash1_phase_angle)) * _yaw_scaler; - _yawFactor[CH_2] = cosf(radians(_servo2_pos + 180 - _swash1_phase_angle)) * _yaw_scaler; - _yawFactor[CH_3] = cosf(radians(_servo3_pos + 180 - _swash1_phase_angle)) * _yaw_scaler; - - _yawFactor[CH_4] = cosf(radians(_servo4_pos - _swash2_phase_angle)) * _yaw_scaler; - _yawFactor[CH_5] = cosf(radians(_servo5_pos - _swash2_phase_angle)) * _yaw_scaler; - _yawFactor[CH_6] = cosf(radians(_servo6_pos - _swash2_phase_angle)) * _yaw_scaler; + // roll tilt + if (swash_axis == AP_MOTORS_HELI_DUAL_SWASH_AXIS_ROLL) { + if (swash_num == 1) { + swash_tilt = 0.0f; + } else if (swash_num == 2) { + swash_tilt = 0.0f; + } + } else if (swash_axis == AP_MOTORS_HELI_DUAL_SWASH_AXIS_PITCH) { + // pitch tilt + if (swash_num == 1) { + swash_tilt = pitch_input - _yaw_scaler * yaw_input; + } else if (swash_num == 2) { + swash_tilt = pitch_input + _yaw_scaler * yaw_input; + } + } else if (swash_axis == AP_MOTORS_HELI_DUAL_SWASH_AXIS_COLL) { + // collective + if (swash_num == 1) { + swash_tilt = 0.45f * _dcp_scaler * roll_input + coll_input; + } else if (swash_num == 2) { + swash_tilt = -0.45f * _dcp_scaler * roll_input + coll_input; + } + } } else { // AP_MOTORS_HELI_DUAL_MODE_TANDEM - // roll factors - _rollFactor[CH_1] = cosf(radians(_servo1_pos + 90 - _swash1_phase_angle)); - _rollFactor[CH_2] = cosf(radians(_servo2_pos + 90 - _swash1_phase_angle)); - _rollFactor[CH_3] = cosf(radians(_servo3_pos + 90 - _swash1_phase_angle)); - - _rollFactor[CH_4] = cosf(radians(_servo4_pos + 90 - _swash2_phase_angle)); - _rollFactor[CH_5] = cosf(radians(_servo5_pos + 90 - _swash2_phase_angle)); - _rollFactor[CH_6] = cosf(radians(_servo6_pos + 90 - _swash2_phase_angle)); - - // pitch factors - _pitchFactor[CH_1] = _dcp_scaler; - _pitchFactor[CH_2] = _dcp_scaler; - _pitchFactor[CH_3] = _dcp_scaler; - - _pitchFactor[CH_4] = -_dcp_scaler; - _pitchFactor[CH_5] = -_dcp_scaler; - _pitchFactor[CH_6] = -_dcp_scaler; - - // yaw factors - _yawFactor[CH_1] = cosf(radians(_servo1_pos + 90 - _swash1_phase_angle)) * _yaw_scaler; - _yawFactor[CH_2] = cosf(radians(_servo2_pos + 90 - _swash1_phase_angle)) * _yaw_scaler; - _yawFactor[CH_3] = cosf(radians(_servo3_pos + 90 - _swash1_phase_angle)) * _yaw_scaler; - - _yawFactor[CH_4] = cosf(radians(_servo4_pos + 270 - _swash2_phase_angle)) * _yaw_scaler; - _yawFactor[CH_5] = cosf(radians(_servo5_pos + 270 - _swash2_phase_angle)) * _yaw_scaler; - _yawFactor[CH_6] = cosf(radians(_servo6_pos + 270 - _swash2_phase_angle)) * _yaw_scaler; - } - - // collective factors - _collectiveFactor[CH_1] = 1; - _collectiveFactor[CH_2] = 1; - _collectiveFactor[CH_3] = 1; - - _collectiveFactor[CH_4] = 1; - _collectiveFactor[CH_5] = 1; - _collectiveFactor[CH_6] = 1; + // roll tilt + if (swash_axis == AP_MOTORS_HELI_DUAL_SWASH_AXIS_ROLL) { + if (swash_num == 1) { + swash_tilt = roll_input + _yaw_scaler * yaw_input; + } else if (swash_num == 2) { + swash_tilt = roll_input - _yaw_scaler * yaw_input; + } + } else if (swash_axis == AP_MOTORS_HELI_DUAL_SWASH_AXIS_PITCH) { + // pitch tilt + if (swash_num == 1) { + swash_tilt = 0.0f; + } else if (swash_num == 2) { + swash_tilt = 0.0f; + } + } else if (swash_axis == AP_MOTORS_HELI_DUAL_SWASH_AXIS_COLL) { + // collective + if (swash_num == 1) { + swash_tilt = 0.45f * _dcp_scaler * pitch_input + coll_input; + } else if (swash_num == 2) { + swash_tilt = -0.45f * _dcp_scaler * pitch_input + coll_input; + } + } + } + return swash_tilt; } // get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used) @@ -377,6 +375,12 @@ uint16_t AP_MotorsHeli_Dual::get_motor_mask() for (uint8_t i=0; i((uint8_t)_swashplate_type)); + _swashplate.set_collective_direction(static_cast((uint8_t)_swash_coll_dir)); + _swashplate.calculate_roll_pitch_collective_factors(); // send setpoints to main rotor controller and trigger recalculation of scalars _main_rotor.set_control_mode(static_cast(_rsc_mode.get())); @@ -292,57 +289,6 @@ void AP_MotorsHeli_Single::calculate_scalars() } } -// CCPM Mixers - calculate mixing scale factors by swashplate type -void AP_MotorsHeli_Single::calculate_roll_pitch_collective_factors() -{ - if (_swash_type == AP_MOTORS_HELI_SINGLE_SWASH_H3) { //Three-Servo adjustable CCPM mixer factors - // aileron factors - _rollFactor[CH_1] = cosf(radians(_servo1_pos + 90 - _phase_angle)); - _rollFactor[CH_2] = cosf(radians(_servo2_pos + 90 - _phase_angle)); - _rollFactor[CH_3] = cosf(radians(_servo3_pos + 90 - _phase_angle)); - - // elevator factors - _pitchFactor[CH_1] = cosf(radians(_servo1_pos - _phase_angle)); - _pitchFactor[CH_2] = cosf(radians(_servo2_pos - _phase_angle)); - _pitchFactor[CH_3] = cosf(radians(_servo3_pos - _phase_angle)); - - // collective factors - _collectiveFactor[CH_1] = 1; - _collectiveFactor[CH_2] = 1; - _collectiveFactor[CH_3] = 1; - } else if (_swash_type == AP_MOTORS_HELI_SINGLE_SWASH_H3_140) { //Three-Servo H3-140 CCPM mixer factors - // aileron factors - _rollFactor[CH_1] = 1; - _rollFactor[CH_2] = -1; - _rollFactor[CH_3] = 0; - - // elevator factors - _pitchFactor[CH_1] = 1; - _pitchFactor[CH_2] = 1; - _pitchFactor[CH_3] = -1; - - // collective factors - _collectiveFactor[CH_1] = 1; - _collectiveFactor[CH_2] = 1; - _collectiveFactor[CH_3] = 1; - } else { //H1 straight outputs, no mixing - // aileron factors - _rollFactor[CH_1] = 1; - _rollFactor[CH_2] = 0; - _rollFactor[CH_3] = 0; - - // elevator factors - _pitchFactor[CH_1] = 0; - _pitchFactor[CH_2] = 1; - _pitchFactor[CH_3] = 0; - - // collective factors - _collectiveFactor[CH_1] = 0; - _collectiveFactor[CH_2] = 0; - _collectiveFactor[CH_3] = 1; - } -} - // get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used) // this can be used to ensure other pwm outputs (i.e. for servos) do not conflict uint16_t AP_MotorsHeli_Single::get_motor_mask() @@ -351,6 +297,10 @@ uint16_t AP_MotorsHeli_Single::get_motor_mask() // setup fast channels uint32_t mask = 1U << 0 | 1U << 1 | 1U << 2 | 1U << 3 | 1U << AP_MOTORS_HELI_SINGLE_RSC; + if (_swashplate_type == SWASHPLATE_TYPE_H4_90 || _swashplate_type == SWASHPLATE_TYPE_H4_45) { + mask |= 1U << 4; + } + if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) { mask |= 1U << AP_MOTORS_HELI_SINGLE_EXTGYRO; } @@ -457,22 +407,13 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float float collective_scalar = ((float)(_collective_max-_collective_min))*0.001f; float collective_out_scaled = collective_out * collective_scalar + (_collective_min - 1000)*0.001f; - // Collective control direction. Swash moves up for negative collective pitch, down for positive collective pitch - if (_collective_direction == AP_MOTORS_HELI_SINGLE_COLLECTIVE_DIRECTION_REVERSED){ - collective_out_scaled = 1 - collective_out_scaled; + // get servo positions from swashplate library + _servo1_out = _swashplate.get_servo_out(CH_1,pitch_out,roll_out,collective_out_scaled); + _servo2_out = _swashplate.get_servo_out(CH_2,pitch_out,roll_out,collective_out_scaled); + _servo3_out = _swashplate.get_servo_out(CH_3,pitch_out,roll_out,collective_out_scaled); + if (_swashplate_type == SWASHPLATE_TYPE_H4_90 || _swashplate_type == SWASHPLATE_TYPE_H4_45) { + _servo5_out = _swashplate.get_servo_out(CH_4,pitch_out,roll_out,collective_out_scaled); } - _servo1_out = ((_rollFactor[CH_1] * roll_out) + (_pitchFactor[CH_1] * pitch_out))*0.45f + _collectiveFactor[CH_1] * collective_out_scaled; - _servo2_out = ((_rollFactor[CH_2] * roll_out) + (_pitchFactor[CH_2] * pitch_out))*0.45f + _collectiveFactor[CH_2] * collective_out_scaled; - if (_swash_type == AP_MOTORS_HELI_SINGLE_SWASH_H1) { - _servo1_out += 0.5f; - _servo2_out += 0.5f; - } - _servo3_out = ((_rollFactor[CH_3] * roll_out) + (_pitchFactor[CH_3] * pitch_out))*0.45f + _collectiveFactor[CH_3] * collective_out_scaled; - - // rescale from -1..1, so we can use the pwm calc that includes trim - _servo1_out = 2*_servo1_out - 1; - _servo2_out = 2*_servo2_out - 1; - _servo3_out = 2*_servo3_out - 1; // update the yaw rate using the tail rotor/servo move_yaw(yaw_out + yaw_offset); @@ -504,6 +445,10 @@ void AP_MotorsHeli_Single::output_to_motors() rc_write_swash(AP_MOTORS_MOT_1, _servo1_out); rc_write_swash(AP_MOTORS_MOT_2, _servo2_out); rc_write_swash(AP_MOTORS_MOT_3, _servo3_out); + // get servo positions from swashplate library and write to servo for 4 servo of 4 servo swashplate + if (_swashplate_type == SWASHPLATE_TYPE_H4_90 || _swashplate_type == SWASHPLATE_TYPE_H4_45) { + rc_write_swash(AP_MOTORS_MOT_5, _servo5_out); + } if (_tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH){ rc_write_angle(AP_MOTORS_MOT_4, _servo4_out * YAW_SERVO_MAX_ANGLE); } @@ -605,10 +550,10 @@ void AP_MotorsHeli_Single::servo_test() // parameter_check - check if helicopter specific parameters are sensible bool AP_MotorsHeli_Single::parameter_check(bool display_msg) const { - // returns false if Phase Angle is outside of range + // returns false if Phase Angle is outside of range for H3 swashplate if ((_phase_angle > 30) || (_phase_angle < -30)){ if (display_msg) { - gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: H_PHANG out of range"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: H_H3_PHANG out of range"); } return false; } diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.h b/libraries/AP_Motors/AP_MotorsHeli_Single.h index efa7008dd4..15aa7956c8 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.h @@ -7,26 +7,13 @@ #include #include "AP_MotorsHeli.h" #include "AP_MotorsHeli_RSC.h" +#include "AP_MotorsHeli_Swash.h" // rsc and extgyro function output channels. #define AP_MOTORS_HELI_SINGLE_RSC CH_8 #define AP_MOTORS_HELI_SINGLE_EXTGYRO CH_7 #define AP_MOTORS_HELI_SINGLE_TAILRSC CH_7 -// servo position defaults -#define AP_MOTORS_HELI_SINGLE_SERVO1_POS -60 -#define AP_MOTORS_HELI_SINGLE_SERVO2_POS 60 -#define AP_MOTORS_HELI_SINGLE_SERVO3_POS 180 - -// swash type definitions -#define AP_MOTORS_HELI_SINGLE_SWASH_H3 0 -#define AP_MOTORS_HELI_SINGLE_SWASH_H1 1 -#define AP_MOTORS_HELI_SINGLE_SWASH_H3_140 2 - -// collective control direction definitions -#define AP_MOTORS_HELI_SINGLE_COLLECTIVE_DIRECTION_NORMAL 0 -#define AP_MOTORS_HELI_SINGLE_COLLECTIVE_DIRECTION_REVERSED 1 - // tail types #define AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO 0 #define AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO 1 @@ -53,7 +40,8 @@ public: uint16_t speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) : AP_MotorsHeli(loop_rate, speed_hz), _main_rotor(SRV_Channel::k_heli_rsc, AP_MOTORS_HELI_SINGLE_RSC), - _tail_rotor(SRV_Channel::k_heli_tail_rsc, AP_MOTORS_HELI_SINGLE_TAILRSC) + _tail_rotor(SRV_Channel::k_heli_tail_rsc, AP_MOTORS_HELI_SINGLE_TAILRSC), + _swashplate() { AP_Param::setup_object_defaults(this, var_info); }; @@ -116,9 +104,6 @@ protected: // update_motor_controls - sends commands to motor controllers void update_motor_control(RotorControlState state) override; - // calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position - void calculate_roll_pitch_collective_factors() override; - // heli_move_actuators - moves swash plate and tail rotor void move_actuators(float roll_out, float pitch_out, float coll_in, float yaw_out) override; @@ -131,6 +116,8 @@ protected: // external objects we depend upon AP_MotorsHeli_RSC _main_rotor; // main rotor AP_MotorsHeli_RSC _tail_rotor; // tail rotor + AP_MotorsHeli_Swash _swashplate; // swashplate + SwashInt16Param _swash_H3; // H3 servo positions for swash // internal variables float _oscillate_angle = 0.0f; // cyclic oscillation angle, used by servo_test function @@ -143,14 +130,12 @@ protected: float _servo2_out = 0.0f; // output value sent to motor float _servo3_out = 0.0f; // output value sent to motor float _servo4_out = 0.0f; // output value sent to motor + float _servo5_out = 0.0f; // output value sent to motor // parameters - AP_Int16 _servo1_pos; // Angular location of swash servo #1 - AP_Int16 _servo2_pos; // Angular location of swash servo #2 - AP_Int16 _servo3_pos; // Angular location of swash servo #3 - AP_Int8 _collective_direction; // Collective control direction, normal or reversed + AP_Int8 _swash_coll_dir; // Collective control direction, normal or reversed AP_Int16 _tail_type; // Tail type used: Servo, Servo with external gyro, direct drive variable pitch or direct drive fixed pitch - AP_Int8 _swash_type; // Swash Type Setting + AP_Int8 _swashplate_type; // Swash Type Setting AP_Int16 _ext_gyro_gain_std; // PWM sent to external gyro on ch7 when tail type is Servo w/ ExtGyro AP_Int16 _ext_gyro_gain_acro; // PWM sent to external gyro on ch7 when tail type is Servo w/ ExtGyro in ACRO AP_Int16 _phase_angle; // Phase angle correction for rotor head. If pitching the swash forward induces a roll, this can be correct the problem @@ -159,7 +144,4 @@ protected: AP_Int16 _direct_drive_tailspeed; // Direct Drive VarPitch Tail ESC speed (0 ~ 1000) bool _acro_tail = false; - float _rollFactor[AP_MOTORS_HELI_SINGLE_NUM_SWASHPLATE_SERVOS]; - float _pitchFactor[AP_MOTORS_HELI_SINGLE_NUM_SWASHPLATE_SERVOS]; - float _collectiveFactor[AP_MOTORS_HELI_SINGLE_NUM_SWASHPLATE_SERVOS]; }; diff --git a/libraries/AP_Motors/AP_MotorsHeli_Swash.cpp b/libraries/AP_Motors/AP_MotorsHeli_Swash.cpp new file mode 100644 index 0000000000..11fc0192fd --- /dev/null +++ b/libraries/AP_Motors/AP_MotorsHeli_Swash.cpp @@ -0,0 +1,183 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include +#include + +#include "AP_MotorsHeli_Swash.h" + +extern const AP_HAL::HAL& hal; + +const AP_Param::GroupInfo SwashInt16Param::var_info[] = { + + // @Param: ENABLE + // @DisplayName: Enable settings for H3 + // @Description: Automatically set when H3 swash type is selected. Should not be set manually. + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + AP_GROUPINFO_FLAGS("ENABLE", 1, SwashInt16Param, enable, 0, AP_PARAM_FLAG_ENABLE), + + // @Param: SV1_POS + // @DisplayName: servo1_pos + // @Description: servo 1 position + // @Range: -180 to 180 + // @Units: deg + // @User: Advanced + AP_GROUPINFO("SV1_POS", 2, SwashInt16Param, servo1_pos, -60), + + // @Param: SV2_POS + // @DisplayName: servo2_pos + // @Description: servo 2 position + // @Range: -180 to 180 + // @Units: deg + // @User: Advanced + AP_GROUPINFO("SV2_POS", 3, SwashInt16Param, servo2_pos, 60), + + // @Param: SV3_POS + // @DisplayName: servo3_pos + // @Description: servo 3 position + // @Range: -180 to 180 + // @Units: deg + // @User: Advanced + AP_GROUPINFO("SV3_POS", 4, SwashInt16Param, servo3_pos, 180), + + // @Param: PHANG + // @DisplayName: Swashplate Phase Angle Compensation + // @Description: Only for H3 swashplate. If pitching the swash forward induces a roll, this can be correct the problem + // @Range: -30 30 + // @Units: deg + // @User: Advanced + // @Increment: 1 + AP_GROUPINFO("PHANG", 5, SwashInt16Param, phase_angle, 0), + + AP_GROUPEND +}; + +/* + a manual swashplate definition with enable and servo position parameters - constructor + */ +SwashInt16Param::SwashInt16Param(void) +{ + AP_Param::setup_object_defaults(this, var_info); +} + +// CCPM Mixers - calculate mixing scale factors by swashplate type +void AP_MotorsHeli_Swash::calculate_roll_pitch_collective_factors() +{ + if (_swash_type == SWASHPLATE_TYPE_H1) { + // CCPM mixing not used + _collectiveFactor[CH_1] = 0; + _collectiveFactor[CH_2] = 0; + _collectiveFactor[CH_3] = 1; + } else if ((_swash_type == SWASHPLATE_TYPE_H4_90) || (_swash_type == SWASHPLATE_TYPE_H4_45)) { + // collective mixer for four-servo CCPM + _collectiveFactor[CH_1] = 1; + _collectiveFactor[CH_2] = 1; + _collectiveFactor[CH_3] = 1; + _collectiveFactor[CH_4] = 1; + } else { + // collective mixer for three-servo CCPM + _collectiveFactor[CH_1] = 1; + _collectiveFactor[CH_2] = 1; + _collectiveFactor[CH_3] = 1; + } + + if (_swash_type == SWASHPLATE_TYPE_H3) { + // Three-servo roll/pitch mixer for adjustable servo position + // can be any style swashplate, phase angle is adjustable + _rollFactor[CH_1] = cosf(radians(_servo1_pos + 90 - _phase_angle)); + _rollFactor[CH_2] = cosf(radians(_servo2_pos + 90 - _phase_angle)); + _rollFactor[CH_3] = cosf(radians(_servo3_pos + 90 - _phase_angle)); + _pitchFactor[CH_1] = cosf(radians(_servo1_pos - _phase_angle)); + _pitchFactor[CH_2] = cosf(radians(_servo2_pos - _phase_angle)); + _pitchFactor[CH_3] = cosf(radians(_servo3_pos - _phase_angle)); + + // defined swashplates, servo1 is always left, servo2 is right, + // servo3 is elevator + } else if (_swash_type == SWASHPLATE_TYPE_H3_140) { // + // Three-servo roll/pitch mixer for H3-140 + // HR3-140 uses reversed servo and collective direction in heli setup + // 1:1 pure input style, phase angle not adjustable + _rollFactor[CH_1] = 1; + _rollFactor[CH_2] = -1; + _rollFactor[CH_3] = 0; + _pitchFactor[CH_1] = 1; + _pitchFactor[CH_2] = 1; + _pitchFactor[CH_3] = -1; + } else if (_swash_type == SWASHPLATE_TYPE_H3_120) { + // three-servo roll/pitch mixer for H3-120 + // HR3-120 uses reversed servo and collective direction in heli setup + // not a pure mixing swashplate, phase angle is adjustable + _rollFactor[CH_1] = 0.866025f; + _rollFactor[CH_2] = -0.866025f; + _rollFactor[CH_3] = 0; + _pitchFactor[CH_1] = 0.5f; + _pitchFactor[CH_2] = 0.5f; + _pitchFactor[CH_3] = -1; + } else if (_swash_type == SWASHPLATE_TYPE_H4_90) { + // four-servo roll/pitch mixer for H4-90 + // 1:1 pure input style, phase angle not adjustable + // servos 3 & 7 are elevator + // can also be used for all versions of 90 deg three-servo swashplates + _rollFactor[CH_1] = 1; + _rollFactor[CH_2] = -1; + _rollFactor[CH_3] = 0; + _rollFactor[CH_4] = 0; + _pitchFactor[CH_1] = 0; + _pitchFactor[CH_2] = 0; + _pitchFactor[CH_3] = -1; + _pitchFactor[CH_4] = 1; + } else if (_swash_type == SWASHPLATE_TYPE_H4_45) { + // four-servo roll/pitch mixer for H4-45 + // 1:1 pure input style, phase angle not adjustable + // for 45 deg plates servos 1&2 are LF&RF, 3&7 are LR&RR. + _rollFactor[CH_1] = 0.707107f; + _rollFactor[CH_2] = -0.707107f; + _rollFactor[CH_3] = 0.707107f; + _rollFactor[CH_4] = -0.707107f; + _pitchFactor[CH_1] = 0.707107f; + _pitchFactor[CH_2] = 0.707107f; + _pitchFactor[CH_3] = -0.707f; + _pitchFactor[CH_4] = -0.707f; + } else { + // CCPM mixing not being used, so H1 straight outputs + _rollFactor[CH_1] = 1; + _rollFactor[CH_2] = 0; + _rollFactor[CH_3] = 0; + _pitchFactor[CH_1] = 0; + _pitchFactor[CH_2] = 1; + _pitchFactor[CH_3] = 0; + } +} + +// get_servo_out - calculates servo output +float AP_MotorsHeli_Swash::get_servo_out(int8_t CH_num, float pitch, float roll, float collective) +{ + // Collective control direction. Swash moves up for negative collective pitch, down for positive collective pitch + if (_collective_direction == COLLECTIVE_DIRECTION_REVERSED){ + collective = 1 - collective; + } + + float servo = ((_rollFactor[CH_num] * roll) + (_pitchFactor[CH_num] * pitch))*0.45f + _collectiveFactor[CH_num] * collective; + if (_swash_type == SWASHPLATE_TYPE_H1 && (CH_num == CH_1 || CH_num == CH_2)) { + servo += 0.5f; + } + + // rescale from -1..1, so we can use the pwm calc that includes trim + servo = 2.0f * servo - 1.0f; + + return servo; +} + diff --git a/libraries/AP_Motors/AP_MotorsHeli_Swash.h b/libraries/AP_Motors/AP_MotorsHeli_Swash.h new file mode 100644 index 0000000000..68873a5a3a --- /dev/null +++ b/libraries/AP_Motors/AP_MotorsHeli_Swash.h @@ -0,0 +1,88 @@ +/// @file AP_MotorsHeli_Swash.h +/// @brief Swashplate Library for traditional heli +#pragma once + +#include +#include // ArduPilot Mega Vector/Matrix math Library +#include + +// swashplate types +enum SwashPlateType { + SWASHPLATE_TYPE_H3 = 0, + SWASHPLATE_TYPE_H1, + SWASHPLATE_TYPE_H3_140, + SWASHPLATE_TYPE_H3_120, + SWASHPLATE_TYPE_H4_90, + SWASHPLATE_TYPE_H4_45 +}; + +// collective direction +enum CollectiveDirection { + COLLECTIVE_DIRECTION_NORMAL = 0, + COLLECTIVE_DIRECTION_REVERSED +}; + +class AP_MotorsHeli_Swash { +public: + friend class AP_MotorsHeli_Single; + friend class AP_MotorsHeli_Dual; + + AP_MotorsHeli_Swash() {}; + + // calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position + void calculate_roll_pitch_collective_factors(); + + // set_swash_type - sets swashplate type + void set_swash_type(SwashPlateType swash_type) { _swash_type = swash_type; } + + // set_collective_direction - sets swashplate collective direction + void set_collective_direction(CollectiveDirection collective_direction) { _collective_direction = collective_direction; } + + // set_phase_angle - sets swashplate phase angle + void set_phase_angle(int16_t phase_angle) { _phase_angle = phase_angle; } + + // set_phase_angle - sets swashplate phase angle + float get_servo_out(int8_t servo_num, float pitch, float roll, float collective); + + void set_servo1_pos(int16_t servo_pos) { _servo1_pos = servo_pos; } + void set_servo2_pos(int16_t servo_pos) { _servo2_pos = servo_pos; } + void set_servo3_pos(int16_t servo_pos) { _servo3_pos = servo_pos; } + void set_servo4_pos(int16_t servo_pos) { _servo4_pos = servo_pos; } + + +private: + // internal variables + SwashPlateType _swash_type; // Swashplate type + CollectiveDirection _collective_direction; // Collective control direction, normal or reversed + int16_t _phase_angle; // Phase angle correction for rotor head. If pitching the swash forward induces a roll, this can be negative depending on mechanics. + float _rollFactor[4]; + float _pitchFactor[4]; + float _collectiveFactor[4]; + int16_t _servo1_pos; + int16_t _servo2_pos; + int16_t _servo3_pos; + int16_t _servo4_pos; + +}; +class SwashInt16Param { +public: + SwashInt16Param(void); + + static const struct AP_Param::GroupInfo var_info[]; + + void set_enable(int8_t setenable) {enable = setenable; } + int8_t get_enable() { return enable; } + int16_t get_servo1_pos() const { return servo1_pos; } + int16_t get_servo2_pos() const { return servo2_pos; } + int16_t get_servo3_pos() const { return servo3_pos; } + int16_t get_phase_angle() const { return phase_angle; } + +private: + AP_Int8 enable; + AP_Int16 servo1_pos; + AP_Int16 servo2_pos; + AP_Int16 servo3_pos; + AP_Int16 phase_angle; + +}; +