From a7038cbaf9e28e92048caf46842a0554b14bf5be Mon Sep 17 00:00:00 2001 From: unknown Date: Sat, 12 Nov 2011 23:23:07 +0900 Subject: [PATCH] TradHeli - new parameter HSV_MAN to allow better set-up from APMissionPlanner. Also changed swash movement to use radio_out which fixes some setup bugs --- ArduCopter/Parameters.h | 5 +++- ArduCopter/heli.pde | 54 ++++++++++++++++++----------------------- ArduCopter/radio.pde | 4 +-- ArduCopter/setup.pde | 31 ++++++++++++----------- ArduCopter/system.pde | 1 + 5 files changed, 46 insertions(+), 49 deletions(-) diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index b1fee3418e..eab4008393 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -75,7 +75,8 @@ public: k_param_heli_collective_mid, k_param_heli_ext_gyro_enabled, k_param_heli_ext_gyro_gain, - k_param_heli_servo_averaging, // 94 + k_param_heli_servo_averaging, + k_param_heli_servo_manual, // 95 #endif // 110: Telemetry control @@ -253,6 +254,7 @@ public: 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 #endif // RC channels @@ -361,6 +363,7 @@ public: heli_ext_gyro_enabled (0, k_param_heli_ext_gyro_enabled, PSTR("GYR_ENABLE_")), 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")), #endif // RC channel group key name diff --git a/ArduCopter/heli.pde b/ArduCopter/heli.pde index 0d0cad434c..b445762ccd 100644 --- a/ArduCopter/heli.pde +++ b/ArduCopter/heli.pde @@ -5,7 +5,6 @@ #define HELI_SERVO_AVERAGING_DIGITAL 0 // 250Hz #define HELI_SERVO_AVERAGING_ANALOG 2 // 125Hz -static int heli_manual_override = false; static float heli_throttle_scaler = 0; // heli_servo_averaging: @@ -81,25 +80,18 @@ 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) -{ +{ // 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); + if( g.heli_servo_manual != 1) { + 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); + } // swashplate servos - g.heli_servo_1.servo_out = (heli_rollFactor[CH_1] * roll_out + heli_pitchFactor[CH_1] * pitch_out)/10 + coll_out + (g.heli_servo_1.radio_trim-1500); - if( g.heli_servo_1.get_reverse() ) - g.heli_servo_1.servo_out = 3000 - g.heli_servo_1.servo_out; - - g.heli_servo_2.servo_out = (heli_rollFactor[CH_2] * roll_out + heli_pitchFactor[CH_2] * pitch_out)/10 + coll_out + (g.heli_servo_2.radio_trim-1500); - if( g.heli_servo_2.get_reverse() ) - g.heli_servo_2.servo_out = 3000 - g.heli_servo_2.servo_out; - - g.heli_servo_3.servo_out = (heli_rollFactor[CH_3] * roll_out + heli_pitchFactor[CH_3] * pitch_out)/10 + coll_out + (g.heli_servo_3.radio_trim-1500); - if( g.heli_servo_3.get_reverse() ) - g.heli_servo_3.servo_out = 3000 - g.heli_servo_3.servo_out; - + 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; // use servo_out to calculate pwm_out and radio_out @@ -109,9 +101,9 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o g.heli_servo_4.calc_pwm(); // add the servo values to the averaging - heli_servo_out[0] += g.heli_servo_1.servo_out; - heli_servo_out[1] += g.heli_servo_2.servo_out; - heli_servo_out[2] += g.heli_servo_3.servo_out; + heli_servo_out[0] += g.heli_servo_1.radio_out; + heli_servo_out[1] += g.heli_servo_2.radio_out; + heli_servo_out[2] += g.heli_servo_3.radio_out; heli_servo_out[3] += g.heli_servo_4.radio_out; heli_servo_out_count++; @@ -125,13 +117,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); @@ -164,19 +156,21 @@ static void init_motors_out() // these are not really motors, they're servos but we don't rename the function because it fits with the rest of the code better static void output_motors_armed() { + // if manual override (i.e. when setting up swash), pass pilot commands straight through to swash + if( g.heli_servo_manual == 1 ) { + g.rc_1.servo_out = g.rc_1.control_in; + g.rc_2.servo_out = g.rc_2.control_in; + 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(); g.rc_3.calc_pwm(); g.rc_4.calc_pwm(); - if( heli_manual_override ) { - // straight pass through from radio inputs to swash plate - heli_move_swash( g.rc_1.control_in, g.rc_2.control_in, g.rc_3.radio_in, g.rc_4.control_in ); - }else{ - // source inputs from attitude controller - heli_move_swash( g.rc_1.servo_out, g.rc_2.servo_out, g.rc_3.radio_out, g.rc_4.servo_out ); - } + heli_move_swash( g.rc_1.servo_out, g.rc_2.servo_out, g.rc_3.radio_out, g.rc_4.servo_out ); } // for helis - armed or disarmed we allow servos to move @@ -200,7 +194,7 @@ static void output_motor_test() 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); + return scaled_throttle; } // heli_angle_boost - takes servo_out position diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index f0b3d11d7a..a5d3e1af28 100644 --- a/ArduCopter/radio.pde +++ b/ArduCopter/radio.pde @@ -9,8 +9,8 @@ static void default_dead_zones() g.rc_1.set_dead_zone(60); g.rc_2.set_dead_zone(60); #if FRAME_CONFIG == HELI_FRAME - g.rc_3.set_dead_zone(0); - g.rc_4.set_dead_zone(60); + g.rc_3.set_dead_zone(20); + g.rc_4.set_dead_zone(30); #else g.rc_3.set_dead_zone(60); g.rc_4.set_dead_zone(200); diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde index 937ea16daa..4ee4179ac5 100644 --- a/ArduCopter/setup.pde +++ b/ArduCopter/setup.pde @@ -469,8 +469,8 @@ setup_heli(uint8_t argc, const Menu::arg *argv) // initialise swash plate heli_init_swash(); - // source swash plate movements directly from - heli_manual_override = true; + // source swash plate movements directly from radio + g.heli_servo_manual = true; // display initial settings report_heli(); @@ -494,6 +494,9 @@ setup_heli(uint8_t argc, const Menu::arg *argv) // read radio although we don't use it yet read_radio(); + + // allow swash plate to move + output_motors_armed(); // record min/max if( state == 1 ) { @@ -501,13 +504,12 @@ setup_heli(uint8_t argc, const Menu::arg *argv) max_roll = abs(g.rc_1.control_in); if( abs(g.rc_2.control_in) > max_pitch ) max_pitch = abs(g.rc_2.control_in); - if( g.rc_3.radio_in < min_coll ) - min_coll = g.rc_3.radio_in; - if( g.rc_3.radio_in > max_coll ) - max_coll = g.rc_3.radio_in; - min_tail = min(g.rc_4.radio_in, min_tail); - max_tail = max(g.rc_4.radio_in, max_tail); - //Serial.printf_P(PSTR("4: ri:%d \tro:%d \tso:%d \n"), (int)g.rc_4.radio_in, (int)g.rc_4.radio_out, (int)g.rc_4.servo_out); + if( g.rc_3.radio_out < min_coll ) + min_coll = g.rc_3.radio_out; + if( g.rc_3.radio_out > max_coll ) + max_coll = g.rc_3.radio_out; + min_tail = min(g.rc_4.radio_out, min_tail); + max_tail = max(g.rc_4.radio_out, max_tail); } if( Serial.available() ) { @@ -533,8 +535,8 @@ setup_heli(uint8_t argc, const Menu::arg *argv) break; case 'c': case 'C': - if( g.rc_3.radio_in >= 900 && g.rc_3.radio_in <= 2100 ) { - g.heli_coll_mid = g.rc_3.radio_in; + if( g.rc_3.radio_out >= 900 && g.rc_3.radio_out <= 2100 ) { + g.heli_coll_mid = g.rc_3.radio_out; Serial.printf_P(PSTR("Collective when blade pitch at zero: %d\n"),(int)g.heli_coll_mid); } break; @@ -561,7 +563,7 @@ setup_heli(uint8_t argc, const Menu::arg *argv) max_pitch = abs(g.rc_2.control_in); min_coll = 2000; max_coll = 1000; - min_tail = max_tail = abs(g.rc_4.radio_in); + min_tail = max_tail = abs(g.rc_4.radio_out); }else{ state = 0; // switch back to normal mode // double check values aren't totally terrible @@ -639,9 +641,6 @@ setup_heli(uint8_t argc, const Menu::arg *argv) } } - // allow swash plate to move - output_motors_armed(); - delay(20); } @@ -664,7 +663,7 @@ setup_heli(uint8_t argc, const Menu::arg *argv) g.heli_servo_averaging.save(); // return swash plate movements to attitude controller - heli_manual_override = false; + g.heli_servo_manual = false; return(0); } diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 3edcd92a55..64746e0b27 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -178,6 +178,7 @@ static void init_ardupilot() #endif #if FRAME_CONFIG == HELI_FRAME + g.heli_servo_manual = false; heli_init_swash(); // heli initialisation #endif