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

TradHeli - added throttle scaling and heli specific angle_boost function that takes into account that heli's main blades can have -ve pitch.

Browse Source
This commit is contained in:
unknown 2011-10-29 17:27:43 +09:00
parent 0211fc2f09
commit ea66c74ff0
4 changed files with 44 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
ArduCopter/ArduCopter.pde
View File

@ -1028,8 +1028,12 @@ void update_throttle_mode(void)
case THROTTLE_MANUAL:
if (g.rc_3.control_in > 0){
angle_boost = get_angle_boost(g.rc_3.control_in);
g.rc_3.servo_out = g.rc_3.control_in + angle_boost;
#if FRAME_CONFIG == HELI_FRAME
g.rc_3.servo_out = heli_get_angle_boost(heli_get_scaled_throttle(g.rc_3.control_in));
#else
angle_boost = get_angle_boost(g.rc_3.control_in);
g.rc_3.servo_out = g.rc_3.control_in + angle_boost;
#endif
}else{
g.pi_stabilize_roll.reset_I();
g.pi_stabilize_pitch.reset_I();
@ -1058,8 +1062,12 @@ void update_throttle_mode(void)
// clear the new data flag
invalid_throttle = false;
}
angle_boost = get_angle_boost(g.throttle_cruise);
g.rc_3.servo_out = g.throttle_cruise + nav_throttle + angle_boost + manual_boost;
#if FRAME_CONFIG == HELI_FRAME
g.rc_3.servo_out = heli_get_angle_boost(g.throttle_cruise + nav_throttle);
#else
angle_boost = get_angle_boost(g.throttle_cruise);
g.rc_3.servo_out = g.throttle_cruise + nav_throttle + angle_boost + manual_boost;
#endif
break;
}
}

25
ArduCopter/heli.pde
View File

@ -6,6 +6,7 @@
#define HELI_SERVO_AVERAGING_ANALOG 2 // 125Hz
static int heli_manual_override = false;
static float heli_throttle_scaler = 0;
// heli_servo_averaging:
// 0 or 1 = no averaging, 250hz
@ -52,6 +53,9 @@ static void heli_init_swash()
g.heli_servo_3.radio_min = g.heli_coll_min - tilt_max[CH_3];
g.heli_servo_3.radio_max = g.heli_coll_max + tilt_max[CH_3];
// scaler for changing channel 3 radio input into collective range
heli_throttle_scaler = ((float)(g.heli_coll_max - g.heli_coll_min))/1000;
// reset the servo averaging
for( i=0; i<=3; i++ )
heli_servo_out[i] = 0;
@ -193,4 +197,25 @@ static void output_motor_test()
{
}
// heli_get_scaled_throttle - user's throttle scaled to collective range
// input is expected to be in the range of 0~1000 (ie. pwm)
// also does equivalent of angle_boost
static int heli_get_scaled_throttle(int throttle)
{
float scaled_throttle = (g.heli_coll_min - 1000) + throttle * heli_throttle_scaler;
return g.heli_coll_min - 1000 + (throttle * heli_throttle_scaler);
}
// heli_angle_boost - takes servo_out position
// adds a boost depending on roll/pitch values
// equivalent of quad's angle_boost function
// pwm_out value should be 0 ~ 1000
static int heli_get_angle_boost(int pwm_out)
{
float angle_boost_factor = cos_pitch_x * cos_roll_x;
angle_boost_factor = 1.0 - constrain(angle_boost_factor, .5, 1.0);
int throttle_above_center = max(1000 + pwm_out - g.heli_coll_mid,0);
return pwm_out + throttle_above_center*angle_boost_factor;
}
#endif // HELI_FRAME

2
ArduCopter/radio.pde
View File

@ -18,7 +18,9 @@ static void init_rc_in()
g.rc_1.set_angle(4500);
g.rc_2.set_angle(4500);
g.rc_3.set_range(0,1000);
#if FRAME_CONFIG != HELI_FRAME
g.rc_3.scale_output = .9;
#endif
g.rc_4.set_angle(4500);
// reverse: CW = left

6
ArduCopter/system.pde
View File

@ -512,7 +512,11 @@ init_throttle_cruise()
// are we moving from manual throttle to auto_throttle?
if((old_control_mode <= STABILIZE) && (g.rc_3.control_in > MINIMUM_THROTTLE)){
g.pi_throttle.reset_I();
g.throttle_cruise.set_and_save(g.rc_3.control_in);
#if FRAME_CONFIG == HELI_FRAME
g.throttle_cruise.set_and_save(heli_get_scaled_throttle(g.rc_3.control_in));
#else
g.throttle_cruise.set_and_save(g.rc_3.control_in);
#endif
}
}