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

Added support for TradHeli H1 swashplate type. Added new param heli_h1_swash_enabled.

Browse Source 
Signed-off-by: Robert Lefebvre <robert.lefebvre@gmail.com>
This commit is contained in:
Robert Lefebvre 2012-03-03 08:14:06 -05:00
parent 064e060264
commit a7e0fc7774
3 changed files with 97 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
ArduCopter/Parameters.h
View File

@ -78,7 +78,8 @@ public:
k_param_heli_servo_averaging,
k_param_heli_servo_manual,
k_param_heli_phase_angle,
k_param_heli_collective_yaw_effect, // 97
k_param_heli_collective_yaw_effect,
k_param_heli_h1_swash_enabled, //98
#endif
// 110: Telemetry control
@ -281,6 +282,7 @@ public:
AP_Int8 heli_servo_manual; // 0 = normal mode, 1 = radio inputs directly control swash. required for swash set-up
AP_Int16 heli_phase_angle; // 0 to 360 degrees. specifies mixing between roll and pitch for helis
AP_Float heli_collective_yaw_effect; // -5.0 ~ 5.0. Feed forward control from collective to yaw. 1.0 = move rudder right 1% for every 1% of collective above the mid point
AP_Int8 heli_h1_swash_enabled; // 0 = CCPM swashplate, 1 = H1 swashplate (no servo mixing)
#endif
// RC channels
@ -387,20 +389,21 @@ public:
auto_slew_rate (AUTO_SLEW_RATE),
#if FRAME_CONFIG == HELI_FRAME
heli_servo1_pos (-60),
heli_servo2_pos (60),
heli_servo3_pos (180),
heli_roll_max (4500),
heli_pitch_max (4500),
heli_collective_min (1250),
heli_collective_max (1750),
heli_collective_mid (1500),
heli_ext_gyro_enabled (0),
heli_ext_gyro_gain (1350),
heli_servo_averaging (0),
heli_servo_manual (0),
heli_phase_angle (0),
heli_servo1_pos (-60),
heli_servo2_pos (60),
heli_servo3_pos (180),
heli_roll_max (4500),
heli_pitch_max (4500),
heli_collective_min (1250),
heli_collective_max (1750),
heli_collective_mid (1500),
heli_ext_gyro_enabled (0),
heli_ext_gyro_gain (1350),
heli_servo_averaging (0),
heli_servo_manual (0),
heli_phase_angle (0),
heli_collective_yaw_effect (0),
heli_h1_swash_enabled (0),
#endif
rc_speed(RC_FAST_SPEED),

21
ArduCopter/Parameters.pde
View File

@ -76,16 +76,17 @@ static const AP_Param::Info var_info[] PROGMEM = {
GGROUP(heli_servo_2, "HS2_", RC_Channel),
GGROUP(heli_servo_3, "HS3_", RC_Channel),
GGROUP(heli_servo_4, "HS4_", RC_Channel),
GSCALAR(heli_servo1_pos, "SV1_POS_"),
GSCALAR(heli_servo2_pos, "SV2_POS_"),
GSCALAR(heli_servo3_pos, "SV3_POS_"),
GSCALAR(heli_roll_max, "ROL_MAX_"),
GSCALAR(heli_pitch_max, "PIT_MAX_"),
GSCALAR(heli_collective_min, "COL_MIN_"),
GSCALAR(heli_collective_max, "COL_MAX_"),
GSCALAR(heli_collective_mid, "COL_MID_"),
GSCALAR(heli_ext_gyro_enabled, "GYR_ENABLE_"),
GSCALAR(heli_ext_gyro_gain, "GYR_GAIN_"),
GSCALAR(heli_servo1_pos, "SV1_POS"),
GSCALAR(heli_servo2_pos, "SV2_POS"),
GSCALAR(heli_servo3_pos, "SV3_POS"),
GSCALAR(heli_roll_max, "ROL_MAX"),
GSCALAR(heli_pitch_max, "PIT_MAX"),
GSCALAR(heli_collective_min, "COL_MIN"),
GSCALAR(heli_collective_max, "COL_MAX"),
GSCALAR(heli_collective_mid, "COL_MID"),
GSCALAR(heli_ext_gyro_enabled, "GYR_ENABLE"),
GSCALAR(heli_h1_swash_enabled, "H1_ENABLE"),
GSCALAR(heli_ext_gyro_gain, "GYR_GAIN"),
GSCALAR(heli_servo_averaging, "SV_AVG"),
GSCALAR(heli_servo_manual, "HSV_MAN"),
GSCALAR(heli_phase_angle, "H_PHANG"),

88
ArduCopter/heli.pde
View File

@ -28,15 +28,40 @@ static void heli_reset_swash()
g.heli_servo_3.radio_min = 1000;
g.heli_servo_3.radio_max = 2000;
// pitch factors
heli_pitchFactor[CH_1] = cos(radians(g.heli_servo1_pos - g.heli_phase_angle));
heli_pitchFactor[CH_2] = cos(radians(g.heli_servo2_pos - g.heli_phase_angle));
heli_pitchFactor[CH_3] = cos(radians(g.heli_servo3_pos - g.heli_phase_angle));
if (!g.heli_h1_swash_enabled){ //CCPM Swashplate, perform servo control mixing
// roll factors
heli_rollFactor[CH_1] = cos(radians(g.heli_servo1_pos + 90 - g.heli_phase_angle));
heli_rollFactor[CH_2] = cos(radians(g.heli_servo2_pos + 90 - g.heli_phase_angle));
heli_rollFactor[CH_3] = cos(radians(g.heli_servo3_pos + 90 - g.heli_phase_angle));
// pitch factors
heli_pitchFactor[CH_1] = cos(radians(g.heli_servo1_pos - g.heli_phase_angle));
heli_pitchFactor[CH_2] = cos(radians(g.heli_servo2_pos - g.heli_phase_angle));
heli_pitchFactor[CH_3] = cos(radians(g.heli_servo3_pos - g.heli_phase_angle));
// collective factors
heli_collectiveFactor[CH_1] = 1;
heli_collectiveFactor[CH_2] = 1;
heli_collectiveFactor[CH_3] = 1;
}else{ //H1 Swashplate, keep servo outputs seperated
// roll factors
heli_rollFactor[CH_1] = cos(radians(g.heli_servo1_pos + 90 - g.heli_phase_angle));
heli_rollFactor[CH_2] = cos(radians(g.heli_servo2_pos + 90 - g.heli_phase_angle));
heli_rollFactor[CH_3] = cos(radians(g.heli_servo3_pos + 90 - g.heli_phase_angle));
// roll factors
heli_rollFactor[CH_1] = 1;
heli_rollFactor[CH_2] = 0;
heli_rollFactor[CH_3] = 0;
// pitch factors
heli_pitchFactor[CH_1] = 0;
heli_pitchFactor[CH_2] = 1;
heli_pitchFactor[CH_3] = 0;
// collective factors
heli_collectiveFactor[CH_1] = 0;
heli_collectiveFactor[CH_2] = 0;
heli_collectiveFactor[CH_3] = 1;
}
// set throttle scaling
heli_collective_scalar = ((float)(g.rc_3.radio_max - g.rc_3.radio_min))/1000.0;
@ -69,15 +94,40 @@ static void heli_init_swash()
// determine scalar throttle input
heli_collective_scalar = ((float)(g.heli_collective_max-g.heli_collective_min))/1000.0;
// pitch factors
heli_pitchFactor[CH_1] = cos(radians(g.heli_servo1_pos - g.heli_phase_angle));
heli_pitchFactor[CH_2] = cos(radians(g.heli_servo2_pos - g.heli_phase_angle));
heli_pitchFactor[CH_3] = cos(radians(g.heli_servo3_pos - g.heli_phase_angle));
if (!g.heli_h1_swash_enabled){ //CCPM Swashplate, perform control mixing
// roll factors
heli_rollFactor[CH_1] = cos(radians(g.heli_servo1_pos + 90 - g.heli_phase_angle));
heli_rollFactor[CH_2] = cos(radians(g.heli_servo2_pos + 90 - g.heli_phase_angle));
heli_rollFactor[CH_3] = cos(radians(g.heli_servo3_pos + 90 - g.heli_phase_angle));
// pitch factors
heli_pitchFactor[CH_1] = cos(radians(g.heli_servo1_pos - g.heli_phase_angle));
heli_pitchFactor[CH_2] = cos(radians(g.heli_servo2_pos - g.heli_phase_angle));
heli_pitchFactor[CH_3] = cos(radians(g.heli_servo3_pos - g.heli_phase_angle));
// collective factors
heli_collectiveFactor[CH_1] = 1;
heli_collectiveFactor[CH_2] = 1;
heli_collectiveFactor[CH_3] = 1;
}else{ //H1 Swashplate, keep servo outputs seperated
// roll factors
heli_rollFactor[CH_1] = cos(radians(g.heli_servo1_pos + 90 - g.heli_phase_angle));
heli_rollFactor[CH_2] = cos(radians(g.heli_servo2_pos + 90 - g.heli_phase_angle));
heli_rollFactor[CH_3] = cos(radians(g.heli_servo3_pos + 90 - g.heli_phase_angle));
// roll factors
heli_rollFactor[CH_1] = 1;
heli_rollFactor[CH_2] = 0;
heli_rollFactor[CH_3] = 0;
// pitch factors
heli_pitchFactor[CH_1] = 0;
heli_pitchFactor[CH_2] = 1;
heli_pitchFactor[CH_3] = 0;
// collective factors
heli_collectiveFactor[CH_1] = 0;
heli_collectiveFactor[CH_2] = 0;
heli_collectiveFactor[CH_3] = 1;
}
// servo min/max values
g.heli_servo_1.radio_min = 1000;
@ -160,9 +210,9 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o
}
// swashplate servos
g.heli_servo_1.servo_out = (heli_rollFactor[CH_1] * roll_out + heli_pitchFactor[CH_1] * pitch_out)/10 + coll_out_scaled + (g.heli_servo_1.radio_trim-1500);
g.heli_servo_2.servo_out = (heli_rollFactor[CH_2] * roll_out + heli_pitchFactor[CH_2] * pitch_out)/10 + coll_out_scaled + (g.heli_servo_2.radio_trim-1500);
g.heli_servo_3.servo_out = (heli_rollFactor[CH_3] * roll_out + heli_pitchFactor[CH_3] * pitch_out)/10 + coll_out_scaled + (g.heli_servo_3.radio_trim-1500);
g.heli_servo_1.servo_out = (heli_rollFactor[CH_1] * roll_out + heli_pitchFactor[CH_1] * pitch_out)/10 + heli_collectiveFactor[CH_1] * coll_out_scaled + (g.heli_servo_1.radio_trim-1500) + g.heli_h1_swash_enabled * 500;
g.heli_servo_2.servo_out = (heli_rollFactor[CH_2] * roll_out + heli_pitchFactor[CH_2] * pitch_out)/10 + heli_collectiveFactor[CH_2] * coll_out_scaled + (g.heli_servo_2.radio_trim-1500) + g.heli_h1_swash_enabled * 500;
g.heli_servo_3.servo_out = (heli_rollFactor[CH_3] * roll_out + heli_pitchFactor[CH_3] * pitch_out)/10 + heli_collectiveFactor[CH_3] * coll_out_scaled + (g.heli_servo_3.radio_trim-1500);
g.heli_servo_4.servo_out = yaw_out + yaw_offset;
// use servo_out to calculate pwm_out and radio_out