From c629795050ca2145ed24edc5a202271aee088f43 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Tue, 3 Jan 2012 10:24:18 -0800 Subject: [PATCH] Cosmetic --- ArduCopter/heli.pde | 40 ++++++++++++++++++++-------------------- 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/ArduCopter/heli.pde b/ArduCopter/heli.pde index d0fcf93949..2f39533114 100644 --- a/ArduCopter/heli.pde +++ b/ArduCopter/heli.pde @@ -41,7 +41,7 @@ static void heli_reset_swash() 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)); @@ -51,12 +51,12 @@ static void heli_reset_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; + heli_swash_initialised = false; } -// initialise the swash +// initialise the swash static void heli_init_swash() { int i; @@ -67,18 +67,18 @@ static void heli_init_swash() 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; g.heli_coll_max = 2000; } 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)); @@ -91,7 +91,7 @@ static void heli_init_swash() 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); @@ -114,7 +114,7 @@ static void heli_init_swash() g.heli_servo_3.radio_min = g.heli_coll_min + (g.heli_servo_3.radio_trim-1500); g.heli_servo_3.radio_max = g.heli_coll_max + (g.heli_servo_3.radio_trim-1500); } - + // reset the servo averaging for( i=0; i<=3; i++ ) heli_servo_out[i] = 0; @@ -124,7 +124,7 @@ static void heli_init_swash() g.heli_servo_averaging = 0; g.heli_servo_averaging.save(); } - + // mark swash as initialised heli_swash_initialised = true; } @@ -147,26 +147,26 @@ static void heli_move_servos_to_mid() // yaw: -4500 ~ 4500 // static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_out) -{ +{ int yaw_offset = 0; - + if( g.heli_servo_manual == 1 ) { // are we in manual servo mode? (i.e. swash set-up mode)? // check if we need to freeup the swash if( heli_swash_initialised ) { heli_reset_swash(); } }else{ // regular flight mode - + // check if we need to reinitialise the swash if( !heli_swash_initialised ) { heli_init_swash(); } - + // 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, 0, 1000); - + // rudder feed forward based on collective #if HIL_MODE == HIL_MODE_DISABLED // don't do rudder feed forward in simulator if( !g.heli_ext_gyro_enabled ) { @@ -193,7 +193,7 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o heli_servo_out[2] += g.heli_servo_3.radio_out; heli_servo_out[3] += g.heli_servo_4.radio_out; heli_servo_out_count++; - + // is it time to move the servos? if( heli_servo_out_count >= g.heli_servo_averaging ) { @@ -204,13 +204,13 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o heli_servo_out[2] /= g.heli_servo_averaging; heli_servo_out[3] /= g.heli_servo_averaging; } - + // actually move the servos APM_RC.OutputCh(CH_1, heli_servo_out[0]); APM_RC.OutputCh(CH_2, heli_servo_out[1]); APM_RC.OutputCh(CH_3, heli_servo_out[2]); APM_RC.OutputCh(CH_4, heli_servo_out[3]); - + // output gyro value if( g.heli_ext_gyro_enabled ) { APM_RC.OutputCh(CH_7, g.heli_ext_gyro_gain); @@ -248,7 +248,7 @@ static void output_motors_armed() g.rc_3.servo_out = g.rc_3.control_in; g.rc_4.servo_out = g.rc_4.control_in; } - + //static int counter = 0; g.rc_1.calc_pwm(); g.rc_2.calc_pwm(); @@ -282,7 +282,7 @@ static int16_t heli_get_angle_boost(int throttle) angle_boost_factor = 1.0 - constrain(angle_boost_factor, .5, 1.0); int throttle_above_mid = max(throttle - heli_throttle_mid,0); return throttle + throttle_above_mid*angle_boost_factor; - + } #endif // HELI_FRAME