diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index d4a2c69f0a..926ab9eef3 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -284,7 +284,8 @@ static bool do_simple = false; #if FRAME_CONFIG == HELI_FRAME static float heli_rollFactor[3], heli_pitchFactor[3]; // only required for 3 swashplate servos static int heli_servo_min[3], heli_servo_max[3]; // same here. for yaw servo we use heli_servo4_min/max parameter directly -static int heli_servo_out[4]; +static long heli_servo_out[4]; // used for servo averaging for analog servos +static int heli_servo_out_count = 0; // use for servo averaging #endif // Failsafe diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 50de454176..d60e42f37e 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -17,7 +17,7 @@ public: // The increment will prevent old parameters from being used incorrectly // by newer code. // - static const uint16_t k_format_version = 108; + static const uint16_t k_format_version = 109; // The parameter software_type is set up solely for ground station use // and identifies the software type (eg ArduPilotMega versus ArduCopterMega) @@ -57,6 +57,27 @@ public: // k_param_log_bitmask, + #if FRAME_CONFIG == HELI_FRAME + // + // 80: Heli + // + k_param_heli_servo_1 = 80, + k_param_heli_servo_2, + k_param_heli_servo_3, + k_param_heli_servo_4, + k_param_heli_servo1_pos , + k_param_heli_servo2_pos, + k_param_heli_servo3_pos, + k_param_heli_roll_max, + k_param_heli_pitch_max, + k_param_heli_collective_min, + k_param_heli_collective_max, + k_param_heli_collective_mid, + k_param_heli_ext_gyro_enabled, + k_param_heli_ext_gyro_gain, + k_param_heli_servo_averaging, // 94 + #endif + // 110: Telemetry control // k_param_streamrates_port0 = 110, @@ -108,28 +129,6 @@ public: k_param_esc_calibrate, k_param_radio_tuning, - - #if FRAME_CONFIG == HELI_FRAME - // - // 200: Heli - // - k_param_heli_servo_1 = 200, - k_param_heli_servo_2, - k_param_heli_servo_3, - k_param_heli_servo_4, - k_param_heli_servo1_pos , - k_param_heli_servo2_pos, - k_param_heli_servo3_pos, - k_param_heli_roll_max, - k_param_heli_pitch_max, - k_param_heli_collective_min, - k_param_heli_collective_max, - k_param_heli_collective_mid, - k_param_heli_ext_gyro_enabled, - k_param_heli_ext_gyro_gain, // 213 - #endif - - // // 210: flight modes // @@ -247,7 +246,8 @@ public: AP_Int16 heli_roll_max, heli_pitch_max; // maximum allowed roll and pitch of swashplate AP_Int16 heli_coll_min, heli_coll_max, heli_coll_mid; // min and max collective. mid = main blades at zero pitch 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_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) #endif // RC channels @@ -349,6 +349,7 @@ public: heli_coll_mid (1500, k_param_heli_collective_mid, PSTR("COL_MID_")), 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")), #endif // RC channel group key name diff --git a/ArduCopter/heli.pde b/ArduCopter/heli.pde index de090eb0a3..fea259b164 100644 --- a/ArduCopter/heli.pde +++ b/ArduCopter/heli.pde @@ -2,8 +2,18 @@ #if FRAME_CONFIG == HELI_FRAME +#define HELI_SERVO_AVERAGING_DIGITAL 0 // 250Hz +#define HELI_SERVO_AVERAGING_ANALOG 2 // 125Hz + static int heli_manual_override = false; -static float rollPitch_impact_on_collective = 0; + +// heli_servo_averaging: +// 0 or 1 = no averaging, 250hz +// 2 = average two samples, 125hz +// 3 = averaging three samples = 83.3 hz +// 4 = averaging four samples = 62.5 hz +// 5 = averaging 5 samples = 50hz +// digital = 0 / 250hz, analog = 2 / 83.3 static void heli_init_swash() { @@ -16,8 +26,6 @@ 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); - //g.heli_servo_4.radio_min = 1000; // required? - //g.heli_servo_4.radio_max = 2000; // pitch factors heli_pitchFactor[CH_1] = cos(radians(g.heli_servo1_pos)); @@ -36,11 +44,6 @@ static void heli_init_swash() total_tilt_max = max(total_tilt_max,tilt_max[i]); } - //if( reset_collective == false ) { - // g.heli_coll_min = total_tilt_max; - // g.heli_coll_max = 1000 - total_tilt_max; - //} - // servo min/max values - or should I use set_range? g.heli_servo_1.radio_min = g.heli_coll_min - tilt_max[CH_1]; g.heli_servo_1.radio_max = g.heli_coll_max + tilt_max[CH_1]; @@ -48,6 +51,16 @@ static void heli_init_swash() g.heli_servo_2.radio_max = g.heli_coll_max + tilt_max[CH_2]; 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]; + + // 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 ) { + g.heli_servo_averaging = 0; + g.heli_servo_averaging.save(); + } } static void heli_move_servos_to_mid() @@ -58,10 +71,10 @@ static void heli_move_servos_to_mid() // // heli_move_swash - moves swash plate to attitude of parameters passed in // - expected ranges: -// roll : -4500 ~ 4500 // should be -500 to 500? +// roll : -4500 ~ 4500 // pitch: -4500 ~ 4500 // collective: 1000 ~ 2000 -// yaw: -4500 ~ 4500?? +// yaw: -4500 ~ 4500 // static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_out) { @@ -94,26 +107,46 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o g.heli_servo_3.calc_pwm(); g.heli_servo_4.calc_pwm(); - // actually move the servos - APM_RC.OutputCh(CH_1, g.heli_servo_1.servo_out); - APM_RC.OutputCh(CH_2, g.heli_servo_2.servo_out); - APM_RC.OutputCh(CH_3, g.heli_servo_3.servo_out); - //APM_RC.OutputCh(CH_4, g.heli_servo_4.servo_out); - APM_RC.OutputCh(CH_4, g.heli_servo_4.radio_out); + // 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[3] += g.heli_servo_4.radio_out; + heli_servo_out_count++; - // output gyro value - if( g.heli_ext_gyro_enabled ) { - APM_RC.OutputCh(CH_7, g.heli_ext_gyro_gain); - } + // is it time to move the servos? + if( heli_servo_out_count >= g.heli_servo_averaging ) { + + // average the values if necessary + if( g.heli_servo_averaging >= 2 ) { + heli_servo_out[0] /= g.heli_servo_averaging; + heli_servo_out[1] /= g.heli_servo_averaging; + 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); + } - // InstantPWM - APM_RC.Force_Out0_Out1(); - APM_RC.Force_Out2_Out3(); - - // debug - //Serial.printf_P(PSTR("4: r%d \tcp:%d \tcol:%d \ty:%d \tout:%d \tpwm:%d \trOut:%d \ttrim:%d\n"), roll_out, pitch_out, coll_out, yaw_out, (int)g.heli_servo_4.servo_out, (int)g.heli_servo_4.pwm_out, (int)g.heli_servo_4.radio_out, (int)g.heli_servo_4.radio_trim); - //Serial.printf_P(PSTR("4: y:%d \tout:%d \tpwm:%d \trOut:%d \ttrim:%d\n"), yaw_out, (int)g.heli_servo_4.servo_out, (int)g.heli_servo_4.pwm_out, (int)g.heli_servo_4.radio_out, (int)g.heli_servo_4.radio_trim); - //Serial.printf_P(PSTR("4: y:%d \tro:%d\n"), yaw_out, (int)g.heli_servo_4.radio_out); + // InstantPWM - force message to the servos + APM_RC.Force_Out0_Out1(); + APM_RC.Force_Out2_Out3(); + + // reset the averaging + heli_servo_out_count = 0; + heli_servo_out[0] = 0; + heli_servo_out[1] = 0; + heli_servo_out[2] = 0; + heli_servo_out[3] = 0; + } } // 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 @@ -128,31 +161,9 @@ static void output_motors_armed() 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 ); - - /*Serial.printf_P( PSTR("1: %d/%d \t2:%d/%d \t3:%d/%d \t4:%d/%d\n"), - (int)g.rc_1.control_in, (int)g.rc_1.servo_out, - (int)g.rc_2.control_in, (int)g.rc_2.servo_out, - (int)g.rc_3.radio_in, (int)g.rc_3.servo_out, - (int)g.rc_4.control_in, (int)g.rc_4.servo_out );*/ }else{ - // collective pitch compensation for yaw/roll. This probably belongs somewhere else - //Matrix3f temp = dcm.get_dcm_matrix(); - //rollPitch_impact_on_collective = 1.0 * (g.rc_3.radio_in-g.heli_coll_mid) * (1.0 - temp.c.z); - //rollPitch_impact_on_collective = constrain(rollPitch_impact_on_collective,0,100); - - /*counter++; - if( counter > 20 ) { - counter = 0; - Serial.printf_P( PSTR("dcm:%f4.1\t rc3:%d\t cm:%d\t imp:%d\n"), - temp.c.z, - (int)g.rc_3.radio_in, - (int)g.heli_coll_mid, - (int)rollPitch_impact_on_collective ); - }*/ - - // source inputs from attitude controller (except for collective pitch) - //heli_move_swash( g.rc_1.servo_out, g.rc_2.servo_out, g.rc_3.radio_in + rollPitch_impact_on_collective, g.rc_4.servo_out ); // to allow control by PIDs except for collective - heli_move_swash( g.rc_1.servo_out, g.rc_2.servo_out, g.rc_3.radio_out, g.rc_4.servo_out ); // to allow control by PIDs except for collective + // 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 ); } } @@ -160,8 +171,7 @@ static void output_motors_armed() static void output_motors_disarmed() { if(g.rc_3.control_in > 0){ - // we have pushed up the throttle - // remove safety + // we have pushed up the throttle, remove safety motor_auto_armed = true; } diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde index e9c25cf4c2..c425ba7f08 100644 --- a/ArduCopter/setup.pde +++ b/ArduCopter/setup.pde @@ -485,6 +485,7 @@ setup_heli(uint8_t argc, const Menu::arg *argv) Serial.printf_P(PSTR("\tm\t\tset roll, pitch, coll min/max\n")); Serial.printf_P(PSTR("\tp\tset pos (i.e. p0 = front, p90 = right)\n")); Serial.printf_P(PSTR("\tr\t\treverse servo\n")); + Serial.printf_P(PSTR("\tu a|d\t\tupdate rate (a=analog servo, d=digital)\n")); Serial.printf_P(PSTR("\tt\tset trim (-500 ~ 500)\n")); Serial.printf_P(PSTR("\tx\t\texit & save\n")); @@ -546,7 +547,7 @@ setup_heli(uint8_t argc, const Menu::arg *argv) case 'M': if( state == 0 ) { state = 1; // switch to capture min/max mode - Serial.printf_P(PSTR("Move coll, roll, pitch and tail to extremes, press 'm' when done\n"),active_servo+1, temp); + Serial.printf_P(PSTR("Move coll, roll, pitch and tail to extremes, press 'm' when done\n")); // reset servo ranges g.heli_roll_max = g.heli_pitch_max = 4500; @@ -611,6 +612,26 @@ setup_heli(uint8_t argc, const Menu::arg *argv) Serial.printf_P(PSTR("Servo %d\t\ttrim:%d\n"),active_servo+1, 1500 + temp); } break; + case 'u': + case 'U': + temp = 0; + // delay up to 2 seconds for servo type from user + while( !Serial.available() && temp < 20 ) { + temp++; + delay(100); + } + if( Serial.available() ) { + value = Serial.read(); + if( value == 'a' || value == 'A' ) { + g.heli_servo_averaging = HELI_SERVO_AVERAGING_ANALOG; + Serial.printf_P(PSTR("Analog Servo %dhz\n"),250 / HELI_SERVO_AVERAGING_ANALOG); + } + if( value == 'd' || value == 'D' ) { + g.heli_servo_averaging = HELI_SERVO_AVERAGING_DIGITAL; + Serial.printf_P(PSTR("Digital Servo 250hz\n")); + } + } + break; case 'z': case 'Z': heli_get_servo(active_servo)->radio_trim -= 10; @@ -640,6 +661,7 @@ setup_heli(uint8_t argc, const Menu::arg *argv) g.heli_coll_min.save(); g.heli_coll_max.save(); g.heli_coll_mid.save(); + g.heli_servo_averaging.save(); // return swash plate movements to attitude controller heli_manual_override = false; @@ -913,6 +935,8 @@ void report_optflow() #if FRAME_CONFIG == HELI_FRAME static void report_heli() { + int servo_rate; + Serial.printf_P(PSTR("Heli\n")); print_divider(); @@ -926,6 +950,14 @@ static void report_heli() Serial.printf_P(PSTR("roll max: \t%d\n"), (int)g.heli_roll_max); Serial.printf_P(PSTR("pitch max: \t%d\n"), (int)g.heli_pitch_max); Serial.printf_P(PSTR("coll min:\t%d\t mid:%d\t max:%d\n"),(int)g.heli_coll_min, (int)g.heli_coll_mid, (int)g.heli_coll_max); + + // calculate and print servo rate + if( g.heli_servo_averaging <= 1 ) { + servo_rate = 250; + } else { + servo_rate = 250 / g.heli_servo_averaging; + } + Serial.printf_P(PSTR("servo rate:\t%d hz\n"),servo_rate); print_blanks(2); }