Slight increase to loop speed - for Max

removed throttle integrator until I have more time to deal with the data
tweaked control loops to be more like RC1 for Max to try. They fly great BTW
tweaked config to be more better.
Mavlink - only cosmetic tweaks

git-svn-id: https://arducopter.googlecode.com/svn/trunk@2254 f9c3cf11-9bcb-44bc-f272-b75c42450872

ArduCopterMega/APM_Config.h

@@ -8,7 +8,7 @@
 
 #define NAV_TEST 0	// 0 = traditional, 1 = rate controlled
 
-#define CHANNEL_6_TUNING CH6_NONE
+#define CHANNEL_6_TUNING CH6_STABLIZE_KD
 //#define CHANNEL_6_TUNING CH6_PMAX
 
 	/*

ArduCopterMega/ArduCopterMega.pde

@@ -459,7 +459,7 @@ void loop()
 {
 	// We want this to execute fast
 	// ----------------------------
-	if (millis() - fast_loopTimer > 6) {
+	if (millis() - fast_loopTimer >= 5) {
 		delta_ms_fast_loop 	= millis() - fast_loopTimer;
 		fast_loopTimer		= millis();
 		load				= float(fast_loopTimeStamp - fast_loopTimer) / delta_ms_fast_loop;
@@ -528,7 +528,7 @@ void fast_loop()
 
 	// record throttle output
 	// ------------------------------
-	throttle_integrator += g.rc_3.servo_out;
+	//throttle_integrator += g.rc_3.servo_out;
 
 	#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && HIL_MODE != HIL_MODE_DISABLED
 		// HIL for a copter needs very fast update of the servo values
@@ -738,6 +738,7 @@ void fifty_hz_loop()
 
 	// kick the GCS to process uplink data
 	gcs.update();
+
 	#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
 		gcs.data_stream_send(45,1000);
 	#endif

ArduCopterMega/Attitude.pde

@@ -49,13 +49,13 @@ output_stabilize_roll()
 	error 		= constrain(error, -2500, 2500);
 
 	// only buildup I if we are trying to roll hard
-	if(abs(g.rc_1.servo_out) < 1000){
+	//if(abs(g.rc_1.servo_out) < 1000){
 		// smoother alternative to reset?
 		//if(g.pid_stabilize_roll.kI() != 0){
 		//	g.pid_stabilize_roll.kI(g.pid_stabilize_roll.kI() * .8);
 		//}
-		g.pid_stabilize_roll.reset_I();
-	}
+	//	g.pid_stabilize_roll.reset_I();
+	//}
 
 	// write out angles back to servo out - this will be converted to PWM by RC_Channel
 	g.rc_1.servo_out 	= g.pid_stabilize_roll.get_pid(error,  	delta_ms_fast_loop, 1.0);		// 2500 * .7 = 1750
@@ -65,9 +65,13 @@ output_stabilize_roll()
 	// omega is the raw gyro reading
 
 	// Limit dampening to be equal to propotional term for symmetry
-	rate				= degrees(omega.x) * 100.0; 													// 6rad = 34377
-	dampener 			= rate * g.stabilize_dampener;										// 34377 * .175 = 6000
-	g.rc_1.servo_out	-= constrain(dampener,  -max_stabilize_dampener, max_stabilize_dampener);	// limit to 1500 based on kP
+	rate				= degrees(omega.x) * 100.0; 												// 6rad = 34377
+	dampener 			= rate * g.stabilize_dampener;												// 34377 * .175 = 6000
+	//g.rc_1.servo_out	-= constrain(dampener,  -max_stabilize_dampener, max_stabilize_dampener);	// limit to 1500 based on kP
+
+	g.rc_1.servo_out	-= dampener;
+	g.rc_1.servo_out	= min(g.rc_1.servo_out, 2500);
+	g.rc_1.servo_out	= max(g.rc_1.servo_out, -2500);
 }
 
 void
@@ -82,9 +86,9 @@ output_stabilize_pitch()
 	error		= constrain(error, -2500, 2500);
 
 	// only buildup I if we are trying to roll hard
-	if(abs(g.rc_2.servo_out) < 1500){
-		g.pid_stabilize_pitch.reset_I();
-	}
+	//if(abs(g.rc_2.servo_out) < 1500){
+	//	g.pid_stabilize_pitch.reset_I();
+	//}
 
 	// write out angles back to servo out - this will be converted to PWM by RC_Channel
 	g.rc_2.servo_out 	= g.pid_stabilize_pitch.get_pid(error, 	delta_ms_fast_loop, 1.0);
@@ -96,7 +100,11 @@ output_stabilize_pitch()
 	// Limit dampening to be equal to propotional term for symmetry
 	rate				= degrees(omega.y) * 100.0; 													// 6rad = 34377
 	dampener 			= rate * g.stabilize_dampener;										// 34377 * .175 = 6000
-	g.rc_2.servo_out	-= constrain(dampener,  -max_stabilize_dampener, max_stabilize_dampener);	// limit to 1500 based on kP
+	//g.rc_2.servo_out	-= constrain(dampener,  -max_stabilize_dampener, max_stabilize_dampener);	// limit to 1500 based on kP
+
+	g.rc_2.servo_out	-= dampener;
+	g.rc_2.servo_out	= min(g.rc_2.servo_out, 2500);
+	g.rc_2.servo_out	= max(g.rc_2.servo_out, -2500);
 }
 
 void
@@ -286,7 +294,7 @@ output_yaw_with_hold(boolean hold)
 		if(g.rc_4.control_in == 0){
 
 			// adaptive braking
-			g.rc_4.servo_out 	= (int)((-3000.0 * omega.z) / 1);
+			g.rc_4.servo_out 	= (int)(-800.0 * omega.z);
 
 			yaw_debug 			= YAW_BRAKE;  // 1
 

ArduCopterMega/GCS_Mavlink.pde

@@ -498,14 +498,15 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             // decode
             mavlink_waypoint_clear_all_t packet;
             mavlink_msg_waypoint_clear_all_decode(msg, &packet);
-            if (mavlink_check_target(packet.target_system,packet.target_component)) break;
+            if (mavlink_check_target(packet.target_system, packet.target_component)) break;
 
             // clear all waypoints
             uint8_t type = 0; // ok (0), error(1)
             g.waypoint_total.set_and_save(0);
 
             // send acknowledgement 3 times to makes sure it is received
-            for (int i=0;i<3;i++) mavlink_msg_waypoint_ack_send(chan,msg->sysid,msg->compid,type);
+            for (int i=0;i<3;i++)
+            	mavlink_msg_waypoint_ack_send(chan, msg->sysid, msg->compid, type);
 
             break;
         }
@@ -540,6 +541,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                 packet.count = MAX_WAYPOINTS;
             }
             g.waypoint_total.set_and_save(packet.count - 1);
+
             waypoint_timelast_receive = millis();
             waypoint_receiving   = true;
             waypoint_sending     = false;
@@ -934,6 +936,7 @@ GCS_MAVLINK::_queued_send()
         (requested_interface == chan) &&
         waypoint_request_i <= g.waypoint_total &&
 		mavdelay > 15) { // limits to 3.33 hz
+
 		mavlink_msg_waypoint_request_send(
 			chan,
             waypoint_dest_sysid,

ArduCopterMega/config.h

@@ -323,29 +323,29 @@
 // STABILZE Angle Control
 //
 #ifndef STABILIZE_ROLL_P
-# define STABILIZE_ROLL_P 		0.6
+# define STABILIZE_ROLL_P 		0.54 // .48 = 4.0 NG, .54 = 4.5 NG
 #endif
 #ifndef STABILIZE_ROLL_I
-# define STABILIZE_ROLL_I 		0.1		//
+# define STABILIZE_ROLL_I 		0.025		//
 #endif
 #ifndef STABILIZE_ROLL_D
-# define STABILIZE_ROLL_D 		0.13
+# define STABILIZE_ROLL_D 		0.12
 #endif
 #ifndef STABILIZE_ROLL_IMAX
-# define STABILIZE_ROLL_IMAX 	10		// 10 degrees
+# define STABILIZE_ROLL_IMAX 	.5		// degrees * 100
 #endif
 
 #ifndef STABILIZE_PITCH_P
-# define STABILIZE_PITCH_P		0.6
+# define STABILIZE_PITCH_P		0.48
 #endif
 #ifndef STABILIZE_PITCH_I
-# define STABILIZE_PITCH_I		0.1
+# define STABILIZE_PITCH_I		0.025
 #endif
 #ifndef STABILIZE_PITCH_D
-# define STABILIZE_PITCH_D		0.13
+# define STABILIZE_PITCH_D		0.12
 #endif
 #ifndef STABILIZE_PITCH_IMAX
-# define STABILIZE_PITCH_IMAX	10
+# define STABILIZE_PITCH_IMAX	.5		// degrees * 100
 #endif
 
 //////////////////////////////////////////////////////////////////////////////

ArduCopterMega/setup.pde

@@ -313,15 +313,15 @@ setup_motors(uint8_t argc, const Menu::arg *argv)
 			}
 
 			g.rc_4.calc_pwm();
-			motor_out[CH_3] 	= g.rc_4.radio_out;
+			// servo Yaw
+			APM_RC.OutputCh(CH_7, g.rc_4.radio_out);
 		}
 
 		if(g.rc_3.control_in > 0){
 			APM_RC.OutputCh(CH_1, g.rc_3.radio_in);
 			APM_RC.OutputCh(CH_2, g.rc_3.radio_in);
 			APM_RC.OutputCh(CH_3, g.rc_3.radio_in);
-			if(g.frame_type != TRI_FRAME)
-				APM_RC.OutputCh(CH_4, g.rc_3.radio_in);
+			APM_RC.OutputCh(CH_4, g.rc_3.radio_in);
 		}else{
 			APM_RC.OutputCh(CH_1, motor_out[CH_1]);
 			APM_RC.OutputCh(CH_2, motor_out[CH_2]);