Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

TradHeli - added two parameters to improve heli control. 

heli_phase_angle allows pilot roll/pitch command to be translated into mixed roll and pitch.
heli_coll_yaw_effect mixes some collective into the yaw so tail can counter act collective movements more easily.
This commit is contained in:
Randy Mackay 2011-11-13 22:20:57 +09:00
parent 930ee51bbe
commit 70aadd4afc
2 changed files with 24 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduCopter/Parameters.h
View File

@ -76,7 +76,9 @@ public:
k_param_heli_ext_gyro_enabled,
k_param_heli_ext_gyro_gain,
k_param_heli_servo_averaging,
k_param_heli_servo_manual, // 95
k_param_heli_servo_manual,
k_param_heli_phase_angle,
k_param_heli_coll_yaw_effect, // 97
#endif
// 110: Telemetry control
@ -248,13 +250,15 @@ public:
#if FRAME_CONFIG == HELI_FRAME
// Heli
RC_Channel heli_servo_1, heli_servo_2, heli_servo_3, heli_servo_4; // servos for swash plate and tail
AP_Int16 heli_servo1_pos, heli_servo2_pos, heli_servo3_pos; // servo positions (3 because we don't need pos for tail servo)
AP_Int16 heli_servo1_pos, heli_servo2_pos, heli_servo3_pos; // servo positions (3 because we don't need pos for tail servo)
AP_Int16 heli_roll_max, heli_pitch_max; // maximum allowed roll and pitch of swashplate
AP_Int16 heli_coll_min, heli_coll_max, heli_coll_mid; // min and max collective. mid = main blades at zero pitch
AP_Int8 heli_ext_gyro_enabled; // 0 = no external tail gyro, 1 = external tail gyro
AP_Int16 heli_ext_gyro_gain; // radio output 1000~2000 (value output on CH_7)
AP_Int8 heli_servo_averaging; // 0 or 1 = no averaging (250hz) for **digital servos**, 2=average of two samples (125hz), 3=three samples (83.3hz) **analog servos**, 4=four samples (62.5hz), 5=5 samples(50hz)
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_coll_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
#endif
// RC channels
@ -364,6 +368,8 @@ public:
heli_ext_gyro_gain (1000, k_param_heli_ext_gyro_gain, PSTR("GYR_GAIN_")),
heli_servo_averaging (0, k_param_heli_servo_averaging, PSTR("SV_AVG")),
heli_servo_manual (0, k_param_heli_servo_manual, PSTR("HSV_MAN")),
heli_phase_angle (0, k_param_heli_phase_angle, PSTR("H_PHANG")),
heli_coll_yaw_effect (0, k_param_heli_coll_yaw_effect, PSTR("H_COLYAW")),
#endif
// RC channel group key name

24
ArduCopter/heli.pde
View File

@ -28,14 +28,14 @@ static void heli_init_swash()
g.heli_servo_4.set_angle(4500);
// pitch factors
heli_pitchFactor[CH_1] = cos(radians(g.heli_servo1_pos));
heli_pitchFactor[CH_2] = cos(radians(g.heli_servo2_pos));
heli_pitchFactor[CH_3] = cos(radians(g.heli_servo3_pos));
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));
heli_rollFactor[CH_2] = cos(radians(g.heli_servo2_pos + 90));
heli_rollFactor[CH_3] = cos(radians(g.heli_servo3_pos + 90));
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));
// collective min / max
total_tilt_max = 0;
@ -81,18 +81,26 @@ static void heli_move_servos_to_mid()
//
static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_out)
{
// ensure values are acceptable:
int yaw_offset = 0;
// regular flight mode checks
if( g.heli_servo_manual != 1) {
// ensure values are acceptable:
roll_out = constrain(roll_out, (int)-g.heli_roll_max, (int)g.heli_roll_max);
pitch_out = constrain(pitch_out, (int)-g.heli_pitch_max, (int)g.heli_pitch_max);
coll_out = constrain(coll_out, (int)g.heli_coll_min, (int)g.heli_coll_max);
// rudder feed forward based on collective
if( !g.heli_ext_gyro_enabled ) {
yaw_offset = g.heli_coll_yaw_effect * (coll_out - g.heli_coll_mid);
}
}
// swashplate servos
g.heli_servo_1.servo_out = (heli_rollFactor[CH_1] * roll_out + heli_pitchFactor[CH_1] * pitch_out)/10 + coll_out - 1000 + (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 - 1000 + (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 - 1000 + (g.heli_servo_3.radio_trim-1500);
g.heli_servo_4.servo_out = yaw_out;
g.heli_servo_4.servo_out = yaw_out + yaw_offset;
// use servo_out to calculate pwm_out and radio_out
g.heli_servo_1.calc_pwm();