TradHeli - fixed roll/pitch so it's not scaled when throttle is scaled

ArduCopter/Attitude.pde

@@ -9,6 +9,17 @@ get_stabilize_roll(int32_t target_angle)
 	// angle error
 	error 		= wrap_180(target_angle - dcm.roll_sensor);
 
+#if FRAME_CONFIG == HELI_FRAME
+	// limit the error we're feeding to the PID
+	error 		= constrain(error, -4500, 4500);
+	
+	// convert to desired Rate:
+	rate 		= g.pi_stabilize_roll.get_pi(error, G_Dt);
+	
+	// output control:
+	rate = constrain(rate, -4500, 4500);
+	return (int)rate;
+#else
 	// limit the error we're feeding to the PID
 	error 		= constrain(error, -2500, 2500);
 
@@ -17,15 +28,15 @@ get_stabilize_roll(int32_t target_angle)
 
 	// experiment to pipe iterm directly into the output
 	int16_t iterm = g.pi_stabilize_roll.get_i(error, G_Dt);
-
+	
-	#if FRAME_CONFIG != HELI_FRAME  // cannot use rate control for helicopters
+	// rate control
 	error 		= rate - (omega.x * DEGX100);
 	rate 		= g.pi_rate_roll.get_pi(error, G_Dt);
-	#endif
 
 	// output control:
 	rate = constrain(rate, -2500, 2500);
 	return (int)rate + iterm;
+#endif
 }
 
 static int
@@ -37,7 +48,18 @@ get_stabilize_pitch(int32_t target_angle)
 	// angle error
 	error 		= wrap_180(target_angle - dcm.pitch_sensor);
 
+#if FRAME_CONFIG == HELI_FRAME
 	// limit the error we're feeding to the PID
+	error 		= constrain(error, -4500, 4500);
+	
+	// convert to desired Rate:
+	rate 		= g.pi_stabilize_pitch.get_pi(error, G_Dt);
+
+	// output control:
+	rate = constrain(rate, -4500, 4500);
+	return (int)rate;
+#else
+	// angle error
 	error 		= constrain(error, -2500, 2500);
 
 	// convert to desired Rate:
@@ -46,14 +68,13 @@ get_stabilize_pitch(int32_t target_angle)
 	// experiment to pipe iterm directly into the output
 	int16_t iterm = g.pi_stabilize_roll.get_i(error, G_Dt);
 
-	#if FRAME_CONFIG != HELI_FRAME  // cannot use rate control for helicopters
 	error 		= rate - (omega.y * DEGX100);
 	rate 		= g.pi_rate_pitch.get_pi(error, G_Dt);
-	#endif
 
 	// output control:
 	rate = constrain(rate, -2500, 2500);
 	return (int)rate + iterm;
+#endif
 }
 
 
@@ -70,21 +91,24 @@ get_stabilize_yaw(int32_t target_angle)
 	// limit the error we're feeding to the PID
 	error 		= constrain(error, -YAW_ERROR_MAX, YAW_ERROR_MAX);
 
-	// convert to desired Rate:
-	rate 		= g.pi_stabilize_yaw.get_p(error);
-
-	// experiment to pipe iterm directly into the output
-	int16_t iterm = g.pi_stabilize_yaw.get_i(error, G_Dt);
-
 #if FRAME_CONFIG == HELI_FRAME  // cannot use rate control for helicopters
+	// convert to desired Rate:
+	rate 		= g.pi_stabilize_yaw.get_pi(error, G_Dt);
+	
 	if(!g.heli_ext_gyro_enabled ) {
 		error 	= rate - (degrees(omega.z) * 100.0);
 		rate 	= g.pi_rate_yaw.get_pi(error, G_Dt);
 	}
 	// output control:
 	rate = constrain(rate, -4500, 4500);
-	return (int)rate + iterm;
+	return (int)rate;
 #else
+	// convert to desired Rate:
+	rate 		= g.pi_stabilize_yaw.get_p(error);
+
+	// experiment to pipe iterm directly into the output
+	int16_t iterm = g.pi_stabilize_yaw.get_i(error, G_Dt);
+	
 	error 		= rate - (omega.z * DEGX100);;
 	rate 		= g.pi_rate_yaw.get_pi(error, G_Dt);
 
@@ -308,8 +332,4 @@ static int get_z_damping()
 	return 0;
 }
 
-#endif
+#endif
-
-
-
-

ArduCopter/heli.pde

@@ -16,16 +16,32 @@ static int heli_throttle_mid = 0;  // throttle mid point in pwm form (i.e. 0 ~ 1
 //   5 = averaging 5 samples = 50hz
 //   digital = 0 / 250hz, analog = 2 / 83.3
 
-static void heli_init_swash()
+// reset swash for maximum movements - used for set-up
+static void heli_reset_swash()
 {
-    int i;
+	// free up servo ranges
-
+	if( g.heli_servo_1.get_reverse() ) {
-	// swash servo initialisation
+		g.heli_servo_1.radio_min = 3000 - g.rc_3.radio_max + (g.heli_servo_1.radio_trim-1500);
-	g.heli_servo_1.set_range(0,1000);
+		g.heli_servo_1.radio_max = 3000 - g.rc_3.radio_min + (g.heli_servo_1.radio_trim-1500);
-	g.heli_servo_2.set_range(0,1000);
+	}else{
-	g.heli_servo_3.set_range(0,1000);
+		g.heli_servo_1.radio_min = g.rc_3.radio_min + (g.heli_servo_1.radio_trim-1500);
-	g.heli_servo_4.set_angle(4500);
+		g.heli_servo_1.radio_max = g.rc_3.radio_max + (g.heli_servo_1.radio_trim-1500);
-
+	}
+	if( g.heli_servo_2.get_reverse() ) {
+		g.heli_servo_2.radio_min = 3000 - g.rc_3.radio_max + (g.heli_servo_2.radio_trim-1500);
+		g.heli_servo_2.radio_max = 3000 - g.rc_3.radio_min + (g.heli_servo_2.radio_trim-1500);
+	}else{
+		g.heli_servo_2.radio_min = g.rc_3.radio_min + (g.heli_servo_2.radio_trim-1500);
+		g.heli_servo_2.radio_max = g.rc_3.radio_max + (g.heli_servo_2.radio_trim-1500);
+	}
+	if( g.heli_servo_3.get_reverse() ) {
+		g.heli_servo_3.radio_min = 3000 - g.rc_3.radio_max + (g.heli_servo_3.radio_trim-1500);
+		g.heli_servo_3.radio_max = 3000 - g.rc_3.radio_min + (g.heli_servo_3.radio_trim-1500);
+	}else{
+		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));
@@ -35,7 +51,23 @@ static void heli_init_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;	
+}
+
+// initialise the swash 
+static void heli_init_swash()
+{
+    int i;
+	float coll_range_comp = 1;   // factor to negate collective range's effect on roll & pitch range
 
+	// swash servo initialisation
+	g.heli_servo_1.set_range(0,1000);
+	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;
@@ -43,6 +75,23 @@ static void heli_init_swash()
 	}
 	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));
+
+	// pitch factors
+	heli_pitchFactor[CH_1] = cos(radians(g.heli_servo1_pos - g.heli_phase_angle)) * coll_range_comp;
+	heli_pitchFactor[CH_2] = cos(radians(g.heli_servo2_pos - g.heli_phase_angle)) * coll_range_comp;
+	heli_pitchFactor[CH_3] = cos(radians(g.heli_servo3_pos - g.heli_phase_angle)) * coll_range_comp;
+
+	// roll factors
+    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);
@@ -66,15 +115,12 @@ static void heli_init_swash()
 		g.heli_servo_3.radio_max = g.heli_coll_max + (g.heli_servo_3.radio_trim-1500);
 	}
 	
-	// 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));
-	
 	// 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 ) {
+	if( g.heli_servo_averaging < 0 || g.heli_servo_averaging > 5 ) {
 	    g.heli_servo_averaging = 0;
 		g.heli_servo_averaging.save();
 	}
@@ -85,7 +131,11 @@ static void heli_init_swash()
 
 static void heli_move_servos_to_mid()
 {
-	heli_move_swash(0,0,500,0);
+    // call multiple times to force through the servo averaging
+    for( int i=0; i<5; i++ ) {
+		heli_move_swash(0,0,500,0);
+		delay(20);
+	}
 }
 
 //
@@ -101,33 +151,10 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o
     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
-	    // we must be in set-up mode so mark swash as uninitialised
+		if( heli_swash_initialised ) {
-	    heli_swash_initialised = false;
+			heli_reset_swash();
-		
-		// free up servo ranges
-		if( g.heli_servo_1.get_reverse() ) {
-			g.heli_servo_1.radio_min = 3000 - g.rc_3.radio_max + (g.heli_servo_1.radio_trim-1500);
-			g.heli_servo_1.radio_max = 3000 - g.rc_3.radio_min + (g.heli_servo_1.radio_trim-1500);
-		}else{
-			g.heli_servo_1.radio_min = g.rc_3.radio_min + (g.heli_servo_1.radio_trim-1500);
-			g.heli_servo_1.radio_max = g.rc_3.radio_max + (g.heli_servo_1.radio_trim-1500);
 		}
-		if( g.heli_servo_2.get_reverse() ) {
-			g.heli_servo_2.radio_min = 3000 - g.rc_3.radio_max + (g.heli_servo_2.radio_trim-1500);
-			g.heli_servo_2.radio_max = 3000 - g.rc_3.radio_min + (g.heli_servo_2.radio_trim-1500);
-		}else{
-			g.heli_servo_2.radio_min = g.rc_3.radio_min + (g.heli_servo_2.radio_trim-1500);
-			g.heli_servo_2.radio_max = g.rc_3.radio_max + (g.heli_servo_2.radio_trim-1500);
-		}
-		if( g.heli_servo_3.get_reverse() ) {
-			g.heli_servo_3.radio_min = 3000 - g.rc_3.radio_max + (g.heli_servo_3.radio_trim-1500);
-			g.heli_servo_3.radio_max = 3000 - g.rc_3.radio_min + (g.heli_servo_3.radio_trim-1500);
-		}else{
-			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);
-		}
-		
 	}else{  // regular flight mode
 		
 		// check if we need to reinitialise the swash
@@ -249,7 +276,7 @@ static void output_motor_test()
 // heli_angle_boost - adds a boost depending on roll/pitch values
 // equivalent of quad's angle_boost function
 // throttle value should be 0 ~ 1000
-static int heli_get_angle_boost(int throttle)
+static int16_t heli_get_angle_boost(int throttle)
 {
     float angle_boost_factor = cos_pitch_x * cos_roll_x;
 	angle_boost_factor = 1.0 - constrain(angle_boost_factor, .5, 1.0);