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

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

Signed-off-by: Robert Lefebvre <robert.lefebvre@gmail.com>
master
Robert Lefebvre 13 years ago
parent
0087d0533a
commit
a480b32400
3 changed files with 99 additions and 45 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. 31
      ArduCopter/Parameters.h
  2. 21
      ArduCopter/Parameters.pde
  3. 92
      ArduCopter/heli.pde

31
ArduCopter/Parameters.h
Unescape View File

@ -78,7 +78,8 @@ public: @@ -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: @@ -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: @@ -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
Unescape View File

@ -76,16 +76,17 @@ static const AP_Param::Info var_info[] PROGMEM = { @@ -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"),

92
ArduCopter/heli.pde
Unescape View File

@ -28,15 +28,40 @@ static void heli_reset_swash() @@ -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));
// 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));
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] = 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() @@ -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));
// 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));
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] = 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 @@ -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

Write Preview
Loading…
Cancel
Save