zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

TradHeli - fixed roll/pitch so it's not scaled when throttle is scaled

mission-4.1.18
Randy Mackay 13 years ago
parent
6dc236b8f2
commit
68e6be5eeb
2 changed files with 105 additions and 58 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. 54
      ArduCopter/Attitude.pde
  2. 109
      ArduCopter/heli.pde

54
ArduCopter/Attitude.pde
Unescape View File

@ -9,6 +9,17 @@ get_stabilize_roll(int32_t target_angle) @@ -9,6 +9,17 @@ get_stabilize_roll(int32_t target_angle)
// angle error
error = wrap_180(target_angle - dcm.roll_sensor);
#if FRAME_CONFIG == HELI_FRAME
// limit the error we're feeding to the PID
error = constrain(error, -4500, 4500);
// convert to desired Rate:
rate = g.pi_stabilize_roll.get_pi(error, G_Dt);
// output control:
rate = constrain(rate, -4500, 4500);
return (int)rate;
#else
// limit the error we're feeding to the PID
error = constrain(error, -2500, 2500);
@ -17,15 +28,15 @@ get_stabilize_roll(int32_t target_angle) @@ -17,15 +28,15 @@ get_stabilize_roll(int32_t target_angle)
// experiment to pipe iterm directly into the output
int16_t iterm = g.pi_stabilize_roll.get_i(error, G_Dt);
#if FRAME_CONFIG != HELI_FRAME // cannot use rate control for helicopters
// rate control
error = rate - (omega.x * DEGX100);
rate = g.pi_rate_roll.get_pi(error, G_Dt);
#endif
// output control:
rate = constrain(rate, -2500, 2500);
return (int)rate + iterm;
#endif
}
static int
@ -37,7 +48,18 @@ get_stabilize_pitch(int32_t target_angle) @@ -37,7 +48,18 @@ get_stabilize_pitch(int32_t target_angle)
// angle error
error = wrap_180(target_angle - dcm.pitch_sensor);
#if FRAME_CONFIG == HELI_FRAME
// limit the error we're feeding to the PID
error = constrain(error, -4500, 4500);
// convert to desired Rate:
rate = g.pi_stabilize_pitch.get_pi(error, G_Dt);
// output control:
rate = constrain(rate, -4500, 4500);
return (int)rate;
#else
// angle error
error = constrain(error, -2500, 2500);
// convert to desired Rate:
@ -46,14 +68,13 @@ get_stabilize_pitch(int32_t target_angle) @@ -46,14 +68,13 @@ get_stabilize_pitch(int32_t target_angle)
// experiment to pipe iterm directly into the output
int16_t iterm = g.pi_stabilize_roll.get_i(error, G_Dt);
#if FRAME_CONFIG != HELI_FRAME // cannot use rate control for helicopters
error = rate - (omega.y * DEGX100);
rate = g.pi_rate_pitch.get_pi(error, G_Dt);
#endif
// output control:
rate = constrain(rate, -2500, 2500);
return (int)rate + iterm;
#endif
}
@ -70,21 +91,24 @@ get_stabilize_yaw(int32_t target_angle) @@ -70,21 +91,24 @@ get_stabilize_yaw(int32_t target_angle)
// limit the error we're feeding to the PID
error = constrain(error, -YAW_ERROR_MAX, YAW_ERROR_MAX);
// convert to desired Rate:
rate = g.pi_stabilize_yaw.get_p(error);
// experiment to pipe iterm directly into the output
int16_t iterm = g.pi_stabilize_yaw.get_i(error, G_Dt);
#if FRAME_CONFIG == HELI_FRAME // cannot use rate control for helicopters
// convert to desired Rate:
rate = g.pi_stabilize_yaw.get_pi(error, G_Dt);
if(!g.heli_ext_gyro_enabled ) {
error = rate - (degrees(omega.z) * 100.0);
rate = g.pi_rate_yaw.get_pi(error, G_Dt);
}
// output control:
rate = constrain(rate, -4500, 4500);
return (int)rate + iterm;
return (int)rate;
#else
// convert to desired Rate:
rate = g.pi_stabilize_yaw.get_p(error);
// experiment to pipe iterm directly into the output
int16_t iterm = g.pi_stabilize_yaw.get_i(error, G_Dt);
error = rate - (omega.z * DEGX100);;
rate = g.pi_rate_yaw.get_pi(error, G_Dt);
@ -308,8 +332,4 @@ static int get_z_damping() @@ -308,8 +332,4 @@ static int get_z_damping()
return 0;
}
#endif
#endif

109
ArduCopter/heli.pde
Unescape View File

@ -16,16 +16,32 @@ static int heli_throttle_mid = 0; // throttle mid point in pwm form (i.e. 0 ~ 1 @@ -16,16 +16,32 @@ static int heli_throttle_mid = 0; // throttle mid point in pwm form (i.e. 0 ~ 1
// 5 = averaging 5 samples = 50hz
// digital = 0 / 250hz, analog = 2 / 83.3
static void heli_init_swash()
// reset swash for maximum movements - used for set-up
static void heli_reset_swash()
{
int i;
// swash servo initialisation
g.heli_servo_1.set_range(0,1000);
g.heli_servo_2.set_range(0,1000);
g.heli_servo_3.set_range(0,1000);
g.heli_servo_4.set_angle(4500);
// free up servo ranges
if( g.heli_servo_1.get_reverse() ) {
g.heli_servo_1.radio_min = 3000 - g.rc_3.radio_max + (g.heli_servo_1.radio_trim-1500);
g.heli_servo_1.radio_max = 3000 - g.rc_3.radio_min + (g.heli_servo_1.radio_trim-1500);
}else{
g.heli_servo_1.radio_min = g.rc_3.radio_min + (g.heli_servo_1.radio_trim-1500);
g.heli_servo_1.radio_max = g.rc_3.radio_max + (g.heli_servo_1.radio_trim-1500);
}
if( g.heli_servo_2.get_reverse() ) {
g.heli_servo_2.radio_min = 3000 - g.rc_3.radio_max + (g.heli_servo_2.radio_trim-1500);
g.heli_servo_2.radio_max = 3000 - g.rc_3.radio_min + (g.heli_servo_2.radio_trim-1500);
}else{
g.heli_servo_2.radio_min = g.rc_3.radio_min + (g.heli_servo_2.radio_trim-1500);
g.heli_servo_2.radio_max = g.rc_3.radio_max + (g.heli_servo_2.radio_trim-1500);
}
if( g.heli_servo_3.get_reverse() ) {
g.heli_servo_3.radio_min = 3000 - g.rc_3.radio_max + (g.heli_servo_3.radio_trim-1500);
g.heli_servo_3.radio_max = 3000 - g.rc_3.radio_min + (g.heli_servo_3.radio_trim-1500);
}else{
g.heli_servo_3.radio_min = g.rc_3.radio_min + (g.heli_servo_3.radio_trim-1500);
g.heli_servo_3.radio_max = g.rc_3.radio_max + (g.heli_servo_3.radio_trim-1500);
}
// 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));
@ -35,7 +51,23 @@ static void heli_init_swash() @@ -35,7 +51,23 @@ static void heli_init_swash()
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));
// we must be in set-up mode so mark swash as uninitialised
heli_swash_initialised = false;
}
// initialise the swash
static void heli_init_swash()
{
int i;
float coll_range_comp = 1; // factor to negate collective range's effect on roll & pitch range
// swash servo initialisation
g.heli_servo_1.set_range(0,1000);
g.heli_servo_2.set_range(0,1000);
g.heli_servo_3.set_range(0,1000);
g.heli_servo_4.set_angle(4500);
// ensure g.heli_coll values are reasonable
if( g.heli_coll_min >= g.heli_coll_max ) {
g.heli_coll_min = 1000;
@ -43,6 +75,23 @@ static void heli_init_swash() @@ -43,6 +75,23 @@ static void heli_init_swash()
}
g.heli_coll_mid = constrain(g.heli_coll_mid, g.heli_coll_min, g.heli_coll_max);
// calculate compensation for collective range on roll & pitch range
if( g.heli_coll_max - g.heli_coll_min > 100 )
coll_range_comp = 1000 / (g.heli_coll_max - g.heli_coll_min);
// calculate throttle mid point
heli_throttle_mid = (g.heli_coll_mid-g.heli_coll_min)*(1000.0/(g.heli_coll_max-g.heli_coll_min));
// pitch factors
heli_pitchFactor[CH_1] = cos(radians(g.heli_servo1_pos - g.heli_phase_angle)) * coll_range_comp;
heli_pitchFactor[CH_2] = cos(radians(g.heli_servo2_pos - g.heli_phase_angle)) * coll_range_comp;
heli_pitchFactor[CH_3] = cos(radians(g.heli_servo3_pos - g.heli_phase_angle)) * coll_range_comp;
// roll factors
heli_rollFactor[CH_1] = cos(radians(g.heli_servo1_pos + 90 - g.heli_phase_angle)) * coll_range_comp;
heli_rollFactor[CH_2] = cos(radians(g.heli_servo2_pos + 90 - g.heli_phase_angle)) * coll_range_comp;
heli_rollFactor[CH_3] = cos(radians(g.heli_servo3_pos + 90 - g.heli_phase_angle)) * coll_range_comp;
// servo min/max values
if( g.heli_servo_1.get_reverse() ) {
g.heli_servo_1.radio_min = 3000 - g.heli_coll_max + (g.heli_servo_1.radio_trim-1500);
@ -66,15 +115,12 @@ static void heli_init_swash() @@ -66,15 +115,12 @@ static void heli_init_swash()
g.heli_servo_3.radio_max = g.heli_coll_max + (g.heli_servo_3.radio_trim-1500);
}
// calculate throttle mid point
heli_throttle_mid = (g.heli_coll_mid-g.heli_coll_min)*(1000.0/(g.heli_coll_max-g.heli_coll_min));
// reset the servo averaging
for( i=0; i<=3; i++ )
heli_servo_out[i] = 0;
// double check heli_servo_averaging is reasonable
if( g.heli_servo_averaging < 0 || g.heli_servo_averaging < 0 > 5 ) {
if( g.heli_servo_averaging < 0 || g.heli_servo_averaging > 5 ) {
g.heli_servo_averaging = 0;
g.heli_servo_averaging.save();
}
@ -85,7 +131,11 @@ static void heli_init_swash() @@ -85,7 +131,11 @@ static void heli_init_swash()
static void heli_move_servos_to_mid()
{
heli_move_swash(0,0,500,0);
// call multiple times to force through the servo averaging
for( int i=0; i<5; i++ ) {
heli_move_swash(0,0,500,0);
delay(20);
}
}
//
@ -101,33 +151,10 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o @@ -101,33 +151,10 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o
int yaw_offset = 0;
if( g.heli_servo_manual == 1 ) { // are we in manual servo mode? (i.e. swash set-up mode)?
// we must be in set-up mode so mark swash as uninitialised
heli_swash_initialised = false;
// free up servo ranges
if( g.heli_servo_1.get_reverse() ) {
g.heli_servo_1.radio_min = 3000 - g.rc_3.radio_max + (g.heli_servo_1.radio_trim-1500);
g.heli_servo_1.radio_max = 3000 - g.rc_3.radio_min + (g.heli_servo_1.radio_trim-1500);
}else{
g.heli_servo_1.radio_min = g.rc_3.radio_min + (g.heli_servo_1.radio_trim-1500);
g.heli_servo_1.radio_max = g.rc_3.radio_max + (g.heli_servo_1.radio_trim-1500);
// check if we need to freeup the swash
if( heli_swash_initialised ) {
heli_reset_swash();
}
if( g.heli_servo_2.get_reverse() ) {
g.heli_servo_2.radio_min = 3000 - g.rc_3.radio_max + (g.heli_servo_2.radio_trim-1500);
g.heli_servo_2.radio_max = 3000 - g.rc_3.radio_min + (g.heli_servo_2.radio_trim-1500);
}else{
g.heli_servo_2.radio_min = g.rc_3.radio_min + (g.heli_servo_2.radio_trim-1500);
g.heli_servo_2.radio_max = g.rc_3.radio_max + (g.heli_servo_2.radio_trim-1500);
}
if( g.heli_servo_3.get_reverse() ) {
g.heli_servo_3.radio_min = 3000 - g.rc_3.radio_max + (g.heli_servo_3.radio_trim-1500);
g.heli_servo_3.radio_max = 3000 - g.rc_3.radio_min + (g.heli_servo_3.radio_trim-1500);
}else{
g.heli_servo_3.radio_min = g.rc_3.radio_min + (g.heli_servo_3.radio_trim-1500);
g.heli_servo_3.radio_max = g.rc_3.radio_max + (g.heli_servo_3.radio_trim-1500);
}
}else{ // regular flight mode
// check if we need to reinitialise the swash
@ -249,7 +276,7 @@ static void output_motor_test() @@ -249,7 +276,7 @@ static void output_motor_test()
// heli_angle_boost - adds a boost depending on roll/pitch values
// equivalent of quad's angle_boost function
// throttle value should be 0 ~ 1000
static int heli_get_angle_boost(int throttle)
static int16_t heli_get_angle_boost(int throttle)
{
float angle_boost_factor = cos_pitch_x * cos_roll_x;
angle_boost_factor = 1.0 - constrain(angle_boost_factor, .5, 1.0);

Write Preview
Loading…
Cancel
Save