zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Motors:Tradheli-swash library parameter scope improvement

master
bnsgeyer 6 years ago committed by Randy Mackay
parent
79b069afc3
commit
01bd99907b
8 changed files with 85 additions and 125 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 6
      libraries/AP_Motors/AP_MotorsHeli.cpp
  2. 1
      libraries/AP_Motors/AP_MotorsHeli.h
  3. 76
      libraries/AP_Motors/AP_MotorsHeli_Dual.cpp
  4. 4
      libraries/AP_Motors/AP_MotorsHeli_Dual.h
  5. 41
      libraries/AP_Motors/AP_MotorsHeli_Single.cpp
  6. 2
      libraries/AP_Motors/AP_MotorsHeli_Single.h
  7. 55
      libraries/AP_Motors/AP_MotorsHeli_Swash.cpp
  8. 23
      libraries/AP_Motors/AP_MotorsHeli_Swash.h

6
libraries/AP_Motors/AP_MotorsHeli.cpp
Unescape View File

@ -193,12 +193,6 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = { @@ -193,12 +193,6 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = {
// @User: Standard
AP_GROUPINFO("RSC_THRCRV_100", 24, AP_MotorsHeli, _rsc_thrcrv[4], AP_MOTORS_HELI_RSC_THRCRV_100_DEFAULT),
// @Param: LIN_SW_SERVO
// @DisplayName: Linearize swashplate servo mechanical throw
// @Description: This linearizes the swashplate servo's mechanical output to account for nonlinear output due to arm rotation. This requires a specific setup procedure to work properly. The servo arm must be centered on the mechanical throw at the servo trim position and the servo trim position kept as close to 1500 as possible. Leveling the swashplate can only be done through the pitch links. See the ardupilot wiki for more details on setup.
// @Values: 0:Disabled,1:Enabled
// @User: Standard
AP_GROUPINFO("LIN_SW_SERVO", 25, AP_MotorsHeli, _linear_swash_servo, 0),
AP_GROUPEND
};

1
libraries/AP_Motors/AP_MotorsHeli.h
Unescape View File

@ -213,7 +213,6 @@ protected: @@ -213,7 +213,6 @@ protected:
AP_Int16 _rsc_thrcrv[5]; // throttle value sent to throttle servo at 0, 25, 50, 75 and 100 percent collective
AP_Int16 _rsc_slewrate; // throttle slew rate (percentage per second)
AP_Int8 _servo_test; // sets number of cycles to test servo movement on bootup
AP_Int8 _linear_swash_servo; // linearize swashplate output
// internal variables
float _collective_mid_pct = 0.0f; // collective mid parameter value converted to 0 ~ 1 range

76
libraries/AP_Motors/AP_MotorsHeli_Dual.cpp
Unescape View File

@ -84,41 +84,15 @@ const AP_Param::GroupInfo AP_MotorsHeli_Dual::var_info[] = { @@ -84,41 +84,15 @@ const AP_Param::GroupInfo AP_MotorsHeli_Dual::var_info[] = {
// @User: Standard
AP_GROUPINFO("COL2_MID", 18, AP_MotorsHeli_Dual, _collective2_mid, AP_MOTORS_HELI_DUAL_COLLECTIVE2_MID),
// @Param: COL_CTRL_DIR
// @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, 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_DIR2", 20, AP_MotorsHeli_Dual, _swash2_coll_dir, COLLECTIVE_DIRECTION_NORMAL),
// @Param: SWASH1_TYPE
// @DisplayName: Swashplate Type for swashplate 1
// @Description: H3 is generic, three-servo only. H3_120/H3_140 plates have Motor1 left side, Motor2 right side, Motor3 elevator in rear. HR3_120/HR3_140 have Motor1 right side, Motor2 left side, Motor3 elevator in front - use H3_120/H3_140 and reverse servo and collective directions as necessary. For all H3_90 swashplates use H4_90 and don't use servo output for the missing servo. For H4-90 Motors1&2 are left/right respectively, Motors3&4 are rear/front respectively. For H4-45 Motors1&2 are LF/RF, Motors3&4 are LR/RR
// @Values: 0:H3 Generic, 1:H1 non-CPPM, 2:H3_140, 3:H3_120, 4:H4_90, 5:H4_45
// @User: Standard
AP_GROUPINFO("SWASH1_TYPE", 21, AP_MotorsHeli_Dual, _swashplate1_type, SWASHPLATE_TYPE_H3),
// @Param: SWASH2_TYPE
// @DisplayName: Swashplate Type for swashplate 2
// @Description: H3 is generic, three-servo only. H3_120/H3_140 plates have Motor1 left side, Motor2 right side, Motor3 elevator in rear. HR3_120/HR3_140 have Motor1 right side, Motor2 left side, Motor3 elevator in front - use H3_120/H3_140 and reverse servo and collective directions as necessary. For all H3_90 swashplates use H4_90 and don't use servo output for the missing servo. For H4-90 Motors1&2 are left/right respectively, Motors3&4 are rear/front respectively. For H4-45 Motors1&2 are LF/RF, Motors3&4 are LR/RR
// @Values: 0:H3 Generic, 1:H1 non-CPPM, 2:H3_140, 3:H3_120, 4:H4_90, 5:H4_45
// @User: Standard
AP_GROUPINFO("SWASH2_TYPE", 22, AP_MotorsHeli_Dual, _swashplate2_type, SWASHPLATE_TYPE_H3),
// Indice 19 was used by COL_CTRL_DIR and should not be used
// @Group: SW1_H3_
// @Path: AP_MotorsHeli_Swash.cpp
AP_SUBGROUPINFO(_swashplate1, "SW1_H3_", 23, AP_MotorsHeli_Dual, AP_MotorsHeli_Swash),
AP_SUBGROUPINFO(_swashplate1, "SW1_", 20, AP_MotorsHeli_Dual, AP_MotorsHeli_Swash),
// @Group: SW2_H3_
// @Path: AP_MotorsHeli_Swash.cpp
AP_SUBGROUPINFO(_swashplate2, "SW2_H3_", 24, AP_MotorsHeli_Dual, AP_MotorsHeli_Swash),
AP_SUBGROUPINFO(_swashplate2, "SW2_", 21, AP_MotorsHeli_Dual, AP_MotorsHeli_Swash),
AP_GROUPEND
};
@ -134,10 +108,10 @@ void AP_MotorsHeli_Dual::set_update_rate( uint16_t speed_hz ) @@ -134,10 +108,10 @@ void AP_MotorsHeli_Dual::set_update_rate( uint16_t speed_hz )
for (uint8_t i=0; i<AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS; i++) {
mask |= 1U << (AP_MOTORS_MOT_1+i);
}
if (_swashplate1_type == SWASHPLATE_TYPE_H4_90 || _swashplate1_type == SWASHPLATE_TYPE_H4_45) {
if (_swashplate1.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate1.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
mask |= 1U << (AP_MOTORS_MOT_7);
}
if (_swashplate2_type == SWASHPLATE_TYPE_H4_90 || _swashplate2_type == SWASHPLATE_TYPE_H4_45) {
if (_swashplate2.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate2.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
mask |= 1U << (AP_MOTORS_MOT_8);
}
@ -152,10 +126,10 @@ bool AP_MotorsHeli_Dual::init_outputs() @@ -152,10 +126,10 @@ bool AP_MotorsHeli_Dual::init_outputs()
for (uint8_t i=0; i<AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS; i++) {
add_motor_num(CH_1+i);
}
if (_swashplate1_type == SWASHPLATE_TYPE_H4_90 || _swashplate1_type == SWASHPLATE_TYPE_H4_45) {
if (_swashplate1.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate1.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
add_motor_num(CH_7);
}
if (_swashplate2_type == SWASHPLATE_TYPE_H4_90 || _swashplate2_type == SWASHPLATE_TYPE_H4_45) {
if (_swashplate2.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate2.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
add_motor_num(CH_8);
}
@ -168,10 +142,10 @@ bool AP_MotorsHeli_Dual::init_outputs() @@ -168,10 +142,10 @@ bool AP_MotorsHeli_Dual::init_outputs()
for (uint8_t i=0; i<AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS; i++) {
reset_swash_servo(SRV_Channels::get_motor_function(i));
}
if (_swashplate1_type == SWASHPLATE_TYPE_H4_90 || _swashplate1_type == SWASHPLATE_TYPE_H4_45) {
if (_swashplate1.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate1.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
reset_swash_servo(SRV_Channels::get_motor_function(6));
}
if (_swashplate2_type == SWASHPLATE_TYPE_H4_90 || _swashplate2_type == SWASHPLATE_TYPE_H4_45) {
if (_swashplate2.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate2.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
reset_swash_servo(SRV_Channels::get_motor_function(7));
}
@ -270,23 +244,11 @@ void AP_MotorsHeli_Dual::calculate_scalars() @@ -270,23 +244,11 @@ void AP_MotorsHeli_Dual::calculate_scalars()
_collective2_mid_pct = ((float)(_collective2_mid-_collective2_min))/((float)(_collective2_max-_collective2_min));
// configure swashplate 1 and update scalars
if (_swashplate1_type == SWASHPLATE_TYPE_H3) {
_swashplate1.set_enable(1);
} else {
_swashplate1.set_enable(0);
}
_swashplate1.set_swash_type(static_cast<SwashPlateType>(_swashplate1_type.get()));
_swashplate1.set_collective_direction(static_cast<CollectiveDirection>(_swash1_coll_dir.get()));
_swashplate1.configure();
_swashplate1.calculate_roll_pitch_collective_factors();
// configure swashplate 2 and update scalars
if (_swashplate2_type == SWASHPLATE_TYPE_H3) {
_swashplate2.set_enable(1);
} else {
_swashplate2.set_enable(0);
}
_swashplate2.set_swash_type(static_cast<SwashPlateType>(_swashplate2_type.get()));
_swashplate2.set_collective_direction(static_cast<CollectiveDirection>(_swash2_coll_dir.get()));
_swashplate2.configure();
_swashplate2.calculate_roll_pitch_collective_factors();
// set mode of main rotor controller and trigger recalculation of scalars
@ -357,10 +319,10 @@ uint16_t AP_MotorsHeli_Dual::get_motor_mask() @@ -357,10 +319,10 @@ uint16_t AP_MotorsHeli_Dual::get_motor_mask()
for (uint8_t i=0; i<AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS; i++) {
mask |= 1U << (AP_MOTORS_MOT_1+i);
}
if (_swashplate1_type == SWASHPLATE_TYPE_H4_90 || _swashplate1_type == SWASHPLATE_TYPE_H4_45) {
if (_swashplate1.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate1.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
mask |= 1U << AP_MOTORS_MOT_7;
}
if (_swashplate2_type == SWASHPLATE_TYPE_H4_90 || _swashplate2_type == SWASHPLATE_TYPE_H4_45) {
if (_swashplate2.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate2.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
mask |= 1U << AP_MOTORS_MOT_8;
}
mask |= 1U << AP_MOTORS_HELI_DUAL_RSC;
@ -497,7 +459,7 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c @@ -497,7 +459,7 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c
_servo_out[CH_1] = _swashplate1.get_servo_out(CH_1,swash1_pitch,swash1_roll,swash1_coll);
_servo_out[CH_2] = _swashplate1.get_servo_out(CH_2,swash1_pitch,swash1_roll,swash1_coll);
_servo_out[CH_3] = _swashplate1.get_servo_out(CH_3,swash1_pitch,swash1_roll,swash1_coll);
if (_swashplate1_type == SWASHPLATE_TYPE_H4_90 || _swashplate1_type == SWASHPLATE_TYPE_H4_45) {
if (_swashplate1.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate1.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
_servo_out[CH_7] = _swashplate1.get_servo_out(CH_4,swash1_pitch,swash1_roll,swash1_coll);
}
@ -505,7 +467,7 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c @@ -505,7 +467,7 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c
_servo_out[CH_4] = _swashplate2.get_servo_out(CH_1,swash2_pitch,swash2_roll,swash2_coll);
_servo_out[CH_5] = _swashplate2.get_servo_out(CH_2,swash2_pitch,swash2_roll,swash2_coll);
_servo_out[CH_6] = _swashplate2.get_servo_out(CH_3,swash2_pitch,swash2_roll,swash2_coll);
if (_swashplate2_type == SWASHPLATE_TYPE_H4_90 || _swashplate2_type == SWASHPLATE_TYPE_H4_45) {
if (_swashplate2.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate2.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
_servo_out[CH_8] = _swashplate2.get_servo_out(CH_4,swash2_pitch,swash2_roll,swash2_coll);
}
@ -522,11 +484,11 @@ void AP_MotorsHeli_Dual::output_to_motors() @@ -522,11 +484,11 @@ void AP_MotorsHeli_Dual::output_to_motors()
}
// write to servo for 4 servo of 4 servo swashplate
if (_swashplate1_type == SWASHPLATE_TYPE_H4_90 || _swashplate1_type == SWASHPLATE_TYPE_H4_45) {
if (_swashplate1.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate1.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
rc_write_swash(AP_MOTORS_MOT_7, _servo_out[CH_7]);
}
// write to servo for 4 servo of 4 servo swashplate
if (_swashplate2_type == SWASHPLATE_TYPE_H4_90 || _swashplate2_type == SWASHPLATE_TYPE_H4_45) {
if (_swashplate2.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate2.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
rc_write_swash(AP_MOTORS_MOT_8, _servo_out[CH_8]);
}
@ -602,7 +564,7 @@ void AP_MotorsHeli_Dual::servo_test() @@ -602,7 +564,7 @@ void AP_MotorsHeli_Dual::servo_test()
bool AP_MotorsHeli_Dual::parameter_check(bool display_msg) const
{
// returns false if Phase Angle is outside of range for H3 swashplate 1
if (_swashplate1_type == SWASHPLATE_TYPE_H3 && (_swashplate1.get_phase_angle() > 30 || _swashplate1.get_phase_angle() < -30)){
if (_swashplate1.get_swash_type() == SWASHPLATE_TYPE_H3 && (_swashplate1.get_phase_angle() > 30 || _swashplate1.get_phase_angle() < -30)){
if (display_msg) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: H_SW1_H3_PHANG out of range");
}
@ -610,7 +572,7 @@ bool AP_MotorsHeli_Dual::parameter_check(bool display_msg) const @@ -610,7 +572,7 @@ bool AP_MotorsHeli_Dual::parameter_check(bool display_msg) const
}
// returns false if Phase Angle is outside of range for H3 swashplate 2
if (_swashplate2_type == SWASHPLATE_TYPE_H3 && (_swashplate2.get_phase_angle() > 30 || _swashplate2.get_phase_angle() < -30)){
if (_swashplate2.get_swash_type() == SWASHPLATE_TYPE_H3 && (_swashplate2.get_phase_angle() > 30 || _swashplate2.get_phase_angle() < -30)){
if (display_msg) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: H_SW2_H3_PHANG out of range");
}

4
libraries/AP_Motors/AP_MotorsHeli_Dual.h
Unescape View File

@ -125,10 +125,6 @@ protected: @@ -125,10 +125,6 @@ protected:
AP_Int16 _collective2_min; // Lowest possible servo position for the rear swashplate
AP_Int16 _collective2_max; // Highest possible servo position for the rear swashplate
AP_Int16 _collective2_mid; // Swash servo position corresponding to zero collective pitch for the rear swashplate (or zero lift for Asymmetrical blades)
AP_Int8 _swashplate1_type; // Swash Type Setting
AP_Int8 _swashplate2_type; // Swash Type Setting
AP_Int8 _swash1_coll_dir; // Collective control direction, normal or reversed
AP_Int8 _swash2_coll_dir; // Collective control direction, normal or reversed
AP_Int8 _dual_mode; // which dual mode the heli is
AP_Float _dcp_scaler; // scaling factor applied to the differential-collective-pitch
AP_Float _dcp_yaw_effect; // feed-forward compensation to automatically add yaw input when differential collective pitch is applied.

41
libraries/AP_Motors/AP_MotorsHeli_Single.cpp
Unescape View File

@ -33,12 +33,7 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] = { @@ -33,12 +33,7 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] = {
// @User: Standard
AP_GROUPINFO("TAIL_TYPE", 4, AP_MotorsHeli_Single, _tail_type, AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO),
// @Param: SWASH_TYPE
// @DisplayName: Swashplate Type
// @Description: H3 is generic, three-servo only. H3_120/H3_140 plates have Motor1 left side, Motor2 right side, Motor3 elevator in rear. HR3_120/HR3_140 have Motor1 right side, Motor2 left side, Motor3 elevator in front - use H3_120/H3_140 and reverse servo and collective directions as necessary. For all H3_90 swashplates use H4_90 and don't use servo output for the missing servo. For H4-90 Motors1&2 are left/right respectively, Motors3&4 are rear/front respectively. For H4-45 Motors1&2 are LF/RF, Motors3&4 are LR/RR
// @Values: 0:H3 Generic, 1:H1 non-CPPM, 2:H3_140, 3:H3_120, 4:H4_90, 5:H4_45
// @User: Standard
AP_GROUPINFO("SWASH_TYPE", 5, AP_MotorsHeli_Single, _swashplate_type, SWASHPLATE_TYPE_H3),
// Indice 5 was used by SWASH_TYPE and should not be used
// @Param: GYR_GAIN
// @DisplayName: External Gyro Gain
@ -84,18 +79,11 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] = { @@ -84,18 +79,11 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] = {
// @User: Standard
AP_GROUPINFO("GYR_GAIN_ACRO", 11, AP_MotorsHeli_Single, _ext_gyro_gain_acro, 0),
// Indices 16-18 were used by RSC_PWM_MIN, RSC_PWM_MAX and RSC_PWM_REV and should not be used
// @Param: COL_CTRL_DIR
// @DisplayName: Collective Control Direction
// @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_Single, _swash_coll_dir, COLLECTIVE_DIRECTION_NORMAL),
// Indices 16-19 were used by RSC_PWM_MIN, RSC_PWM_MAX, RSC_PWM_REV, and COL_CTRL_DIR and should not be used
// @Group: H3_SW_
// @Path: AP_MotorsHeli_Swash.cpp
AP_SUBGROUPINFO(_swashplate, "SW_H3_", 20, AP_MotorsHeli_Single, AP_MotorsHeli_Swash),
AP_SUBGROUPINFO(_swashplate, "SW_", 20, AP_MotorsHeli_Single, AP_MotorsHeli_Swash),
AP_GROUPEND
};
@ -114,7 +102,7 @@ void AP_MotorsHeli_Single::set_update_rate( uint16_t speed_hz ) @@ -114,7 +102,7 @@ void AP_MotorsHeli_Single::set_update_rate( uint16_t speed_hz )
1U << AP_MOTORS_MOT_2 |
1U << AP_MOTORS_MOT_3 |
1U << AP_MOTORS_MOT_4;
if (_swashplate_type == SWASHPLATE_TYPE_H4_90 || _swashplate_type == SWASHPLATE_TYPE_H4_45) {
if (_swashplate.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
mask |= 1U << (AP_MOTORS_MOT_5);
}
rc_set_freq(mask, _speed_hz);
@ -128,7 +116,7 @@ bool AP_MotorsHeli_Single::init_outputs() @@ -128,7 +116,7 @@ bool AP_MotorsHeli_Single::init_outputs()
for (uint8_t i=0; i<AP_MOTORS_HELI_SINGLE_NUM_SWASHPLATE_SERVOS; i++) {
add_motor_num(CH_1+i);
}
if (_swashplate_type == SWASHPLATE_TYPE_H4_90 || _swashplate_type == SWASHPLATE_TYPE_H4_45) {
if (_swashplate.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
add_motor_num(CH_5);
}
@ -155,7 +143,7 @@ bool AP_MotorsHeli_Single::init_outputs() @@ -155,7 +143,7 @@ bool AP_MotorsHeli_Single::init_outputs()
for (uint8_t i=0; i<AP_MOTORS_HELI_SINGLE_NUM_SWASHPLATE_SERVOS; i++) {
reset_swash_servo(SRV_Channels::get_motor_function(i));
}
if (_swashplate_type == SWASHPLATE_TYPE_H4_90 || _swashplate_type == SWASHPLATE_TYPE_H4_45) {
if (_swashplate.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
reset_swash_servo(SRV_Channels::get_motor_function(4));
}
@ -250,15 +238,8 @@ void AP_MotorsHeli_Single::calculate_scalars() @@ -250,15 +238,8 @@ void AP_MotorsHeli_Single::calculate_scalars()
_collective_mid_pct = ((float)(_collective_mid-_collective_min))/((float)(_collective_max-_collective_min));
// configure swashplate and update scalars
if (_swashplate_type == SWASHPLATE_TYPE_H3) {
_swashplate.set_enable(1);
} else {
_swashplate.set_enable(0);
}
_swashplate.set_swash_type(static_cast<SwashPlateType>(_swashplate_type.get()));
_swashplate.set_collective_direction(static_cast<CollectiveDirection>(_swash_coll_dir.get()));
_swashplate.configure();
_swashplate.calculate_roll_pitch_collective_factors();
_swashplate.set_linear_servo_out(_linear_swash_servo);
// send setpoints to main rotor controller and trigger recalculation of scalars
_main_rotor.set_control_mode(static_cast<RotorControlMode>(_rsc_mode.get()));
@ -288,7 +269,7 @@ uint16_t AP_MotorsHeli_Single::get_motor_mask() @@ -288,7 +269,7 @@ 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) {
if (_swashplate.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
mask |= 1U << 4;
}
@ -402,7 +383,7 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float @@ -402,7 +383,7 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float
_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) {
if (_swashplate.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
_servo5_out = _swashplate.get_servo_out(CH_4,pitch_out,roll_out,collective_out_scaled);
}
@ -437,7 +418,7 @@ void AP_MotorsHeli_Single::output_to_motors() @@ -437,7 +418,7 @@ void AP_MotorsHeli_Single::output_to_motors()
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) {
if (_swashplate.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
rc_write_swash(AP_MOTORS_MOT_5, _servo5_out);
}
if (_tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH){
@ -542,7 +523,7 @@ void AP_MotorsHeli_Single::servo_test() @@ -542,7 +523,7 @@ void AP_MotorsHeli_Single::servo_test()
bool AP_MotorsHeli_Single::parameter_check(bool display_msg) const
{
// returns false if Phase Angle is outside of range for H3 swashplate
if (_swashplate_type == SWASHPLATE_TYPE_H3 && (_swashplate.get_phase_angle() > 30 || _swashplate.get_phase_angle() < -30)){
if (_swashplate.get_swash_type() == SWASHPLATE_TYPE_H3 && (_swashplate.get_phase_angle() > 30 || _swashplate.get_phase_angle() < -30)){
if (display_msg) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: H_H3_PHANG out of range");
}

2
libraries/AP_Motors/AP_MotorsHeli_Single.h
Unescape View File

@ -132,9 +132,7 @@ protected: @@ -132,9 +132,7 @@ protected:
float _servo5_out = 0.0f; // output value sent to motor
// parameters
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 _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_Float _collective_yaw_effect; // Feed-forward compensation to automatically add rudder input when collective pitch is increased. Can be positive or negative depending on mechanics.

55
libraries/AP_Motors/AP_MotorsHeli_Swash.cpp
Unescape View File

@ -22,49 +22,84 @@ extern const AP_HAL::HAL& hal; @@ -22,49 +22,84 @@ extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AP_MotorsHeli_Swash::var_info[] = {
// @Param: ENABLE
// @Param: TYPE
// @DisplayName: Swashplate Type
// @Description: H3 is generic, three-servo only. H3_120/H3_140 plates have Motor1 left side, Motor2 right side, Motor3 elevator in rear. HR3_120/HR3_140 have Motor1 right side, Motor2 left side, Motor3 elevator in front - use H3_120/H3_140 and reverse servo and collective directions as necessary. For all H3_90 swashplates use H4_90 and don't use servo output for the missing servo. For H4-90 Motors1&2 are left/right respectively, Motors3&4 are rear/front respectively. For H4-45 Motors1&2 are LF/RF, Motors3&4 are LR/RR
// @Values: 0:H3 Generic, 1:H1 non-CPPM, 2:H3_140, 3:H3_120, 4:H4_90, 5:H4_45
// @User: Standard
AP_GROUPINFO("TYPE", 1, AP_MotorsHeli_Swash, _swashplate_type, SWASHPLATE_TYPE_H3),
// @Param: COL_DIR
// @DisplayName: Collective Control Direction
// @Description: Direction collective moves for positive pitch. 0 for Normal, 1 for Reversed
// @Values: 0:Normal,1:Reversed
// @User: Standard
AP_GROUPINFO("COL_DIR", 2, AP_MotorsHeli_Swash, _swash_coll_dir, COLLECTIVE_DIRECTION_NORMAL),
// @Param: LIN_SVO
// @DisplayName: Linearize swashplate servo mechanical throw
// @Description: This linearizes the swashplate servo's mechanical output to account for nonlinear output due to arm rotation. This requires a specific setup procedure to work properly. The servo arm must be centered on the mechanical throw at the servo trim position and the servo trim position kept as close to 1500 as possible. Leveling the swashplate can only be done through the pitch links. See the ardupilot wiki for more details on setup.
// @Values: 0:Disabled,1:Enabled
// @User: Standard
AP_GROUPINFO("LIN_SVO", 3, AP_MotorsHeli_Swash, _linear_swash_servo, 0),
// @Param: H3_ENABLE
// @DisplayName: Enable generic H3 swashplate settings
// @Description: Automatically set when H3 generic swash type is selected. Do not set manually.
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
AP_GROUPINFO_FLAGS("ENABLE", 1, AP_MotorsHeli_Swash, enable, 0, AP_PARAM_FLAG_ENABLE),
AP_GROUPINFO_FLAGS("H3_ENABLE", 4, AP_MotorsHeli_Swash, enable, 0, AP_PARAM_FLAG_ENABLE),
// @Param: SV1_POS
// @Param: H3_SV1_POS
// @DisplayName: servo 1 position
// @Description: Azimuth position on swashplate for servo 1 with the front of the heli being 0 deg
// @Range: -180 180
// @Units: deg
// @User: Advanced
AP_GROUPINFO("SV1_POS", 2, AP_MotorsHeli_Swash, _servo1_pos, -60),
AP_GROUPINFO("H3_SV1_POS", 5, AP_MotorsHeli_Swash, _servo1_pos, -60),
// @Param: SV2_POS
// @Param: H3_SV2_POS
// @DisplayName: servo 2 position
// @Description: Azimuth position on swashplate for servo 2 with the front of the heli being 0 deg
// @Range: -180 180
// @Units: deg
// @User: Advanced
AP_GROUPINFO("SV2_POS", 3, AP_MotorsHeli_Swash, _servo2_pos, 60),
AP_GROUPINFO("H3_SV2_POS", 6, AP_MotorsHeli_Swash, _servo2_pos, 60),
// @Param: SV3_POS
// @Param: H3_SV3_POS
// @DisplayName: servo 3 position
// @Description: Azimuth position on swashplate for servo 3 with the front of the heli being 0 deg
// @Range: -180 180
// @Units: deg
// @User: Advanced
AP_GROUPINFO("SV3_POS", 4, AP_MotorsHeli_Swash, _servo3_pos, 180),
AP_GROUPINFO("H3_SV3_POS", 7, AP_MotorsHeli_Swash, _servo3_pos, 180),
// @Param: PHANG
// @Param: H3_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, AP_MotorsHeli_Swash, _phase_angle, 0),
AP_GROUPINFO("H3_PHANG", 8, AP_MotorsHeli_Swash, _phase_angle, 0),
AP_GROUPEND
};
// configure - configure the swashplate settings for any updated parameters
void AP_MotorsHeli_Swash::configure()
{
_swash_type = static_cast<SwashPlateType>(_swashplate_type.get());
_collective_direction = static_cast<CollectiveDirection>(_swash_coll_dir.get());
_make_servo_linear = _linear_swash_servo;
if (_swash_type == SWASHPLATE_TYPE_H3) {
enable = 1;
} else {
enable = 0;
}
}
// CCPM Mixers - calculate mixing scale factors by swashplate type
void AP_MotorsHeli_Swash::calculate_roll_pitch_collective_factors()
{

23
libraries/AP_Motors/AP_MotorsHeli_Swash.h
Unescape View File

@ -24,36 +24,28 @@ enum CollectiveDirection { @@ -24,36 +24,28 @@ enum CollectiveDirection {
class AP_MotorsHeli_Swash {
public:
friend class AP_MotorsHeli_Single;
friend class AP_MotorsHeli_Dual;
AP_MotorsHeli_Swash()
{
AP_Param::setup_object_defaults(this, var_info);
};
// configure - configure the swashplate settings for any updated parameters
void configure();
// CCPM Mixers - calculate mixing scale factors by swashplate type
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; }
// get_swash_type - gets swashplate type
SwashPlateType get_swash_type() const { return _swash_type; }
// get_servo_out - calculates servo output
float get_servo_out(int8_t servo_num, float pitch, float roll, float collective) const;
// set_linear_servo_out - sets swashplate servo output to be linear
void set_linear_servo_out(int8_t linear_servo) { _make_servo_linear = linear_servo; }
// linearize mechanical output of swashplate servo
float get_linear_servo_output(float input) const;
// allow parameters to be enabled
void set_enable(int8_t setenable) {enable = setenable; }
// get_phase_angle - returns the rotor phase angle which is used to remove coupling between pitch and roll axes
// get_phase_angle - returns the rotor phase angle
int16_t get_phase_angle() const { return _phase_angle; }
// var_info
@ -69,6 +61,9 @@ private: @@ -69,6 +61,9 @@ private:
int8_t _make_servo_linear; // Sets servo output to be linearized
// parameters
AP_Int8 _swashplate_type; // Swash Type Setting
AP_Int8 _swash_coll_dir; // Collective control direction, normal or reversed
AP_Int8 _linear_swash_servo; // linearize swashplate output
AP_Int8 enable;
AP_Int16 _servo1_pos; // servo1 azimuth position on swashplate with front of heli being 0 deg
AP_Int16 _servo2_pos; // servo2 azimuth position on swashplate with front of heli being 0 deg

Write Preview
Loading…
Cancel
Save