ArduCopter - added PID log type. Implemented for Yaw stabilize and rate controllers.

ArduCopter/Attitude.pde

@@ -59,30 +59,47 @@ get_stabilize_pitch(int32_t target_angle)
 static int16_t
 get_stabilize_yaw(int32_t target_angle)
 {
-	// angle error
+	static int8_t log_counter = 0;
-	target_angle 		= wrap_180(target_angle - ahrs.yaw_sensor);
+	int32_t target_rate,i_term;
+	int32_t angle_error;
+	int32_t output;
+
+	// angle error
+	angle_error	 	= wrap_180(target_angle - ahrs.yaw_sensor);
 
-#if FRAME_CONFIG == HELI_FRAME  // cannot use rate control for helicopters
 	// limit the error we're feeding to the PID
-	target_angle 		= constrain(target_angle, -4500, 4500);
+#if FRAME_CONFIG == HELI_FRAME
+	angle_error 		= constrain(angle_error, -4500, 4500);
 #else
-	// limit the error we're feeding to the PID
+	angle_error 		= constrain(angle_error, -2000, 2000);
-	target_angle 		= constrain(target_angle, -2000, 2000);
 #endif
 
-	// conver to desired Rate:
+	// convert angle error to desired Rate:
-	int32_t target_rate = g.pi_stabilize_yaw.get_p(target_angle);
+	target_rate = g.pi_stabilize_yaw.get_p(angle_error);
-	int16_t iterm 		= g.pi_stabilize_yaw.get_i(target_angle, G_Dt);
+	i_term = g.pi_stabilize_yaw.get_i(angle_error, G_Dt);
 
-#if FRAME_CONFIG == HELI_FRAME  // cannot use rate control for helicopters
+	// do not use rate controllers for helicotpers with external gyros
+#if FRAME_CONFIG == HELI_FRAME
 	if(!g.heli_ext_gyro_enabled){
-		return get_rate_yaw(target_rate) + iterm;
+		output = get_rate_yaw(target_rate) + iterm;
 	}else{
-		return constrain((target_rate + iterm), -4500, 4500);
+		output = constrain((target_rate + i_term), -4500, 4500);
 	}
 #else
-	return get_rate_yaw(target_rate) + iterm;
+	output = get_rate_yaw(target_rate) + i_term;
 #endif
+
+	// log output if PID logging is on and we are tuning the yaw
+	if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_YAW_KP || g.radio_tuning == CH6_YAW_RATE_KP) ) {
+		log_counter++;
+		if( log_counter >= 10 ) {	// (update rate / desired output rate) = (100hz / 10hz) = 10
+			log_counter = 0;
+			Log_Write_PID(CH6_YAW_KP, angle_error, target_rate, i_term, 0, output, g.pi_stabilize_yaw.kP());
+		}
+	}
+
+	// ensure output does not go beyond barries of what an int16_t can hold
+	return constrain(output,-32000,32000);
 }
 
 static int16_t
@@ -172,15 +189,38 @@ get_rate_pitch(int32_t target_rate)
 static int16_t
 get_rate_yaw(int32_t target_rate)
 {
+	static int8_t log_counter = 0;
+	int32_t p,i,d;
+	int32_t rate_error;
+	int32_t output;
+
 	// rate control
-	target_rate	 	= target_rate - (omega.z * DEGX100);
+	rate_error	 	= target_rate - (omega.z * DEGX100);
-	target_rate 	= g.pid_rate_yaw.get_pid(target_rate, G_Dt);
+
+	// separately calculate p, i, d values for logging
+	p = g.pid_rate_yaw.get_p(rate_error);
+	i = g.pid_rate_yaw.get_i(rate_error, G_Dt);
+	d = g.pid_rate_yaw.get_d(rate_error, G_Dt);
+
+	output 	= p+i+d;
 
 	// output control:
 	int16_t yaw_limit = 1400 + abs(g.rc_4.control_in);
 
-	// smoother Yaw control:
+	// constrain output
-	return constrain(target_rate, -yaw_limit, yaw_limit);
+	output = constrain(output, -yaw_limit, yaw_limit);
+
+	// log output if PID loggins is on and we are tuning the yaw
+	if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_YAW_KP || g.radio_tuning == CH6_YAW_RATE_KP) ) {
+		log_counter++;
+		if( log_counter >= 10 ) {	// (update rate / desired output rate) = (100hz / 10hz) = 10
+			log_counter = 0;
+			Log_Write_PID(CH6_YAW_RATE_KP, rate_error, p, i, d, output, g.pid_rate_yaw.kP());
+		}
+	}
+
+	// constrain output
+	return output;
 }
 
 static int16_t

ArduCopter/Log.pde

@@ -72,6 +72,7 @@ print_log_menu(void)
 		if (g.log_bitmask & MASK_LOG_CUR)			Serial.printf_P(PSTR(" CURRENT"));
 		if (g.log_bitmask & MASK_LOG_MOTORS)		Serial.printf_P(PSTR(" MOTORS"));
 		if (g.log_bitmask & MASK_LOG_OPTFLOW)		Serial.printf_P(PSTR(" OPTFLOW"));
+		if (g.log_bitmask & MASK_LOG_PID)			Serial.printf_P(PSTR(" PID"));
 	}
 
 	Serial.println();
@@ -193,6 +194,7 @@ select_logs(uint8_t argc, const Menu::arg *argv)
 		TARG(CUR);
 		TARG(MOTORS);
 		TARG(OPTFLOW);
+		TARG(PID);
 		#undef TARG
 	}
 
@@ -783,6 +785,46 @@ static void Log_Read_Data()
 	Serial.printf_P(PSTR("DATA: %d, %ld\n"), temp1, temp2);
 }
 
+// Write an PID packet. Total length : 28 bytes
+static void Log_Write_PID(int8_t pid_id, int32_t error, int32_t p, int32_t i, int32_t d, int32_t output, float gain)
+{
+	DataFlash.WriteByte(HEAD_BYTE1);
+	DataFlash.WriteByte(HEAD_BYTE2);
+	DataFlash.WriteByte(LOG_PID_MSG);
+
+	DataFlash.WriteByte(pid_id);			// 1
+	DataFlash.WriteLong(error);				// 2
+	DataFlash.WriteLong(p);					// 3
+	DataFlash.WriteLong(i);					// 4
+	DataFlash.WriteLong(d);					// 5
+	DataFlash.WriteLong(output);			// 6
+	DataFlash.WriteLong(gain * 1000);		// 7
+
+	DataFlash.WriteByte(END_BYTE);
+}
+
+// Read a PID packet
+static void Log_Read_PID()
+{
+	int8_t temp1 	= DataFlash.ReadByte();		// pid id
+	int32_t temp2 	= DataFlash.ReadLong();		// error
+	int32_t temp3 	= DataFlash.ReadLong();		// p
+	int32_t temp4 	= DataFlash.ReadLong();		// i
+	int32_t temp5 	= DataFlash.ReadLong();		// d
+	int32_t temp6 	= DataFlash.ReadLong();		// output
+	float temp7 	= DataFlash.ReadLong() / 1000.f;		// gain
+
+						   //  1    2    3    4    5    6      7
+	Serial.printf_P(PSTR("PID-%d, %ld, %ld, %ld, %ld, %ld, %4.4f\n"),
+                    (int)temp1,		// pid id
+                    (long)temp2,	// error
+                    (long)temp3,	// p
+                    (long)temp4,	// i
+                    (long)temp5,	// d
+                    (long)temp6,	// output
+                    temp7);			// gain
+}
+
 // Read the DataFlash log memory
 static void Log_Read(int start_page, int end_page)
 {
@@ -791,6 +833,7 @@ static void Log_Read(int start_page, int end_page)
 	#ifdef AIRFRAME_NAME
 		Serial.printf_P(PSTR((AIRFRAME_NAME)
 	#endif
+
 	Serial.printf_P(PSTR("\n" THISFIRMWARE
 						 "\nFree RAM: %u\n"),
                     memcheck_available_memory());
@@ -890,6 +933,10 @@ static int Log_Read_Process(int start_page, int end_page)
 					case LOG_DATA_MSG:
 						Log_Read_Data();
 						break;
+
+					case LOG_PID_MSG:
+						Log_Read_PID();
+						break;
 				}
 				break;
 			case 3:
@@ -925,6 +972,7 @@ static void Log_Write_Nav_Tuning() {}
 static void Log_Write_Control_Tuning() {}
 static void Log_Write_Motors() {}
 static void Log_Write_Performance() {}
+static void Log_Write_PID() {}
 static int8_t process_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
 
 #endif // LOGGING_DISABLED

ArduCopter/config.h

@@ -830,27 +830,31 @@
 #ifndef LOG_MOTORS
 # define LOG_MOTORS				DISABLED
 #endif
-// guess!
+// optical flow
 #ifndef LOG_OPTFLOW
 # define LOG_OPTFLOW			DISABLED
 #endif
+#ifndef LOG_PID
+# define LOG_PID				DISABLED
+#endif
 
 // calculate the default log_bitmask
 #define LOGBIT(_s)     (LOG_##_s ? MASK_LOG_##_s : 0)
 
 #define DEFAULT_LOG_BITMASK \
-               LOGBIT(ATTITUDE_FAST)	| \
+			LOGBIT(ATTITUDE_FAST)	| \
-               LOGBIT(ATTITUDE_MED)		| \
+			LOGBIT(ATTITUDE_MED)	| \
-               LOGBIT(GPS)				| \
+			LOGBIT(GPS)				| \
-               LOGBIT(PM)               | \
+			LOGBIT(PM)				| \
-               LOGBIT(CTUN)				| \
+			LOGBIT(CTUN)			| \
-               LOGBIT(NTUN)				| \
+			LOGBIT(NTUN)			| \
-               LOGBIT(MODE)             | \
+			LOGBIT(MODE)			| \
-               LOGBIT(RAW)              | \
+			LOGBIT(RAW)				| \
-               LOGBIT(CMD)              | \
+			LOGBIT(CMD)				| \
-               LOGBIT(CUR)				| \
+			LOGBIT(CUR)				| \
-               LOGBIT(MOTORS)			| \
+			LOGBIT(MOTORS)			| \
-               LOGBIT(OPTFLOW)
+			LOGBIT(OPTFLOW)			| \
+			LOGBIT(PID)
 
 // if we are using fast, Disable Medium
 //#if LOG_ATTITUDE_FAST == ENABLED

ArduCopter/defines.h

@@ -265,6 +265,7 @@ enum gcs_severity {
 #define LOG_MOTORS_MSG 			0x0B
 #define LOG_OPTFLOW_MSG 		0x0C
 #define LOG_DATA_MSG 			0x0D
+#define LOG_PID_MSG 			0x0E
 #define LOG_INDEX_MSG			0xF0
 #define MAX_NUM_LOGS			50
 
@@ -280,6 +281,7 @@ enum gcs_severity {
 #define MASK_LOG_CUR			(1<<9)
 #define MASK_LOG_MOTORS			(1<<10)
 #define MASK_LOG_OPTFLOW		(1<<11)
+#define MASK_LOG_PID			(1<<12)
 
 // Waypoint Modes
 // ----------------