aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Motors:TradHeli - add support for reverse collective swashplates

Browse Source
This commit is contained in:
ChristopherOlson 2018-03-28 10:20:59 -05:00 committed by Randy Mackay
parent f3c9761901
commit f073c58f68
4 changed files with 42 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_Motors/AP_MotorsHeli_Dual.cpp
View File

@ -152,6 +152,13 @@ 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
// @Description: Collective Control Direction - 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_GROUPEND
};
@ -494,6 +501,12 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c
float collective2_scaler = ((float)(_collective2_max-_collective2_min))*0.001f;
float collective2_out_scaled = collective2_out * collective2_scaler + (_collective2_min - 1000)*0.001f;
// Collective control direction. Swash plates move up for negative collective pitch, down for positive collective pitch
if (_collective_direction == AP_MOTORS_HELI_DUAL_COLLECTIVE_DIRECTION_REVERSED){
collective_out_scaled = 1 - collective_out_scaled;
collective2_out_scaled = 1 - collective2_out_scaled;
}
// feed power estimate into main rotor controller
// ToDo: add main rotor cyclic power?
_rotor.set_collective(fabsf(collective_out));

5
libraries/AP_Motors/AP_MotorsHeli_Dual.h
View File

@ -20,6 +20,10 @@
#define AP_MOTORS_HELI_DUAL_SERVO5_POS 60
#define AP_MOTORS_HELI_DUAL_SERVO6_POS 180
// collective control direction definitions
#define AP_MOTORS_HELI_DUAL_COLLECTIVE_DIRECTION_NORMAL 0
#define AP_MOTORS_HELI_DUAL_COLLECTIVE_DIRECTION_REVERSED 1
// rsc function output channel
#define AP_MOTORS_HELI_DUAL_RSC CH_8
@ -124,6 +128,7 @@ protected:
AP_Int16 _servo4_pos; // angular location of swash servo #4
AP_Int16 _servo5_pos; // angular location of swash servo #5
AP_Int16 _servo6_pos; // angular location of swash servo #6
AP_Int8 _collective_direction; // Collective control direction, normal or reversed
AP_Int16 _swash1_phase_angle; // phase angle correction for 1st swash.
AP_Int16 _swash2_phase_angle; // phase angle correction for 2nd swash.
AP_Int8 _dual_mode; // which dual mode the heli is

24
libraries/AP_Motors/AP_MotorsHeli_Single.cpp
View File

@ -116,6 +116,13 @@ 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),
// @Param: COL_CTRL_DIR
// @DisplayName: Collective Control Direction
// @Description: Collective Control Direction - 0 for Normal. 1 for Reversed
// @Values: 0: Normal, 1: Reversed
// @User: Standard
AP_GROUPINFO("COL_CTRL_DIR", 19, AP_MotorsHeli_Single, _collective_direction, AP_MOTORS_HELI_SINGLE_COLLECTIVE_DIRECTION_NORMAL),
// Indices 16-18 were used by RSC_PWM_MIN, RSC_PWM_MAX and RSC_PWM_REV and should not be used
// parameters up to and including 29 are reserved for tradheli
@ -232,7 +239,7 @@ void AP_MotorsHeli_Single::calculate_armed_scalars()
float thrcrv[5];
for (uint8_t i = 0; i < 5; i++) {
thrcrv[i]=_rsc_thrcrv[i]*0.001f;
}
}
_main_rotor.set_ramp_time(_rsc_ramp_time);
_main_rotor.set_runup_time(_rsc_runup_time);
_main_rotor.set_critical_speed(_rsc_critical*0.001f);
@ -415,16 +422,21 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float
// ToDo: add main rotor cyclic power?
_main_rotor.set_collective(fabsf(collective_out));
// swashplate servos
// scale collective pitch for swashplate servos
float collective_scalar = ((float)(_collective_max-_collective_min))*0.001f;
float coll_out_scaled = collective_out * collective_scalar + (_collective_min - 1000)*0.001f;
float servo1_out = ((_rollFactor[CH_1] * roll_out) + (_pitchFactor[CH_1] * pitch_out))*0.45f + _collectiveFactor[CH_1] * coll_out_scaled;
float servo2_out = ((_rollFactor[CH_2] * roll_out) + (_pitchFactor[CH_2] * pitch_out))*0.45f + _collectiveFactor[CH_2] * coll_out_scaled;
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;
}
float servo1_out = ((_rollFactor[CH_1] * roll_out) + (_pitchFactor[CH_1] * pitch_out))*0.45f + _collectiveFactor[CH_1] * collective_out_scaled;
float 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;
}
float servo3_out = ((_rollFactor[CH_3] * roll_out) + (_pitchFactor[CH_3] * pitch_out))*0.45f + _collectiveFactor[CH_3] * coll_out_scaled;
float 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;

7
libraries/AP_Motors/AP_MotorsHeli_Single.h
View File

@ -21,6 +21,10 @@
#define AP_MOTORS_HELI_SINGLE_SWASH_CCPM 0
#define AP_MOTORS_HELI_SINGLE_SWASH_H1 1
// 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
@ -137,7 +141,8 @@ protected:
// 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_Int16 _servo3_pos; // Angular location of swash servo #3
AP_Int8 _collective_direction; // 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 - either 3-servo CCPM or H1 Mechanical Mixing
AP_Int16 _ext_gyro_gain_std; // PWM sent to external gyro on ch7 when tail type is Servo w/ ExtGyro