Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 09:58:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
rmackay9 2012-03-25 15:55:49 +09:00
parent 0451da1d43
commit 441413f1c6
4 changed files with 124 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

74
ArduCopter/Attitude.pde
View File

@ -59,30 +59,47 @@ get_stabilize_pitch(int32_t target_angle)
static int16_t
get_stabilize_yaw(int32_t target_angle)
{
// angle error
target_angle = wrap_180(target_angle - ahrs.yaw_sensor);
static int8_t log_counter = 0;
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
target_angle = constrain(target_angle, -2000, 2000);
angle_error = constrain(angle_error, -2000, 2000);
#endif
// conver to desired Rate:
int32_t target_rate = g.pi_stabilize_yaw.get_p(target_angle);
int16_t iterm = g.pi_stabilize_yaw.get_i(target_angle, G_Dt);
// convert angle error to desired Rate:
target_rate = g.pi_stabilize_yaw.get_p(angle_error);
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);
target_rate = g.pid_rate_yaw.get_pid(target_rate, G_Dt);
rate_error = target_rate - (omega.z * DEGX100);
// 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:
return constrain(target_rate, -yaw_limit, yaw_limit);
// constrain output
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

48
ArduCopter/Log.pde
View File

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

30
ArduCopter/config.h
View File

@ -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_MED) | \
LOGBIT(GPS) | \
LOGBIT(PM) | \
LOGBIT(CTUN) | \
LOGBIT(NTUN) | \
LOGBIT(MODE) | \
LOGBIT(RAW) | \
LOGBIT(CMD) | \
LOGBIT(CUR) | \
LOGBIT(MOTORS) | \
LOGBIT(OPTFLOW)
LOGBIT(ATTITUDE_FAST) | \
LOGBIT(ATTITUDE_MED) | \
LOGBIT(GPS) | \
LOGBIT(PM) | \
LOGBIT(CTUN) | \
LOGBIT(NTUN) | \
LOGBIT(MODE) | \
LOGBIT(RAW) | \
LOGBIT(CMD) | \
LOGBIT(CUR) | \
LOGBIT(MOTORS) | \
LOGBIT(OPTFLOW) | \
LOGBIT(PID)
// if we are using fast, Disable Medium
//#if LOG_ATTITUDE_FAST == ENABLED

2
ArduCopter/defines.h
View File

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