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

@@ -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

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

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

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

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