From ea66c74ff094aacd0955a9f3b9192a5e4071d403 Mon Sep 17 00:00:00 2001 From: unknown Date: Sat, 29 Oct 2011 17:27:43 +0900 Subject: [PATCH] TradHeli - added throttle scaling and heli specific angle_boost function that takes into account that heli's main blades can have -ve pitch. --- ArduCopter/ArduCopter.pde | 16 ++++++++++++---- ArduCopter/heli.pde | 25 +++++++++++++++++++++++++ ArduCopter/radio.pde | 2 ++ ArduCopter/system.pde | 6 +++++- 4 files changed, 44 insertions(+), 5 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 721d3e3938..94ad2fd033 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -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; } } diff --git a/ArduCopter/heli.pde b/ArduCopter/heli.pde index 873212e119..4dd3719b29 100644 --- a/ArduCopter/heli.pde +++ b/ArduCopter/heli.pde @@ -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 diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index 649ef66774..cf5abebca0 100644 --- a/ArduCopter/radio.pde +++ b/ArduCopter/radio.pde @@ -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 diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index c284938fb2..2f7d46e5fe 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -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 } }