TradHeli - added servo averaging to allow support of analog servos

- moved heli parameters to 80 because they were causing flight modes to overlap with waypoints

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

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

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;
 	}
 

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<angle>\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<angle>\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);
 }