From df9d1614e26daed747b0fda93ef9451643d5056c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 13 Jan 2014 17:23:09 +0900 Subject: [PATCH] Copter: remove PID logging Almost never used and eats up limited log description space --- ArduCopter/ArduCopter.pde | 2 - ArduCopter/Attitude.pde | 85 --------------------------------------- ArduCopter/Log.pde | 31 -------------- ArduCopter/defines.h | 4 +- ArduCopter/heli.pde | 13 +----- 5 files changed, 3 insertions(+), 132 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 6edc5f86f8..5071b2a272 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -482,8 +482,6 @@ static MOTOR_CLASS motors(&g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4); static Vector3f omega; // This is used to hold radio tuning values for in-flight CH6 tuning float tuning_value; -// used to limit the rate that the pid controller output is logged so that it doesn't negatively affect performance -static uint8_t pid_log_counter; //////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 0b2916099b..fce43e5d5a 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -70,7 +70,6 @@ get_stabilize_yaw(int32_t target_angle) { int32_t target_rate; int32_t angle_error; - int32_t output = 0; // angle error angle_error = wrap_180_cd(target_angle - ahrs.yaw_sensor); @@ -88,17 +87,6 @@ get_stabilize_yaw(int32_t target_angle) } #endif -#if LOGGING_ENABLED == ENABLED - // log output if PID logging is on and we are tuning the yaw - if( g.log_bitmask & MASK_LOG_PID && g.radio_tuning == CH6_STABILIZE_YAW_KP ) { - pid_log_counter++; - if( pid_log_counter >= 10 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10 - pid_log_counter = 0; - Log_Write_PID(CH6_STABILIZE_YAW_KP, angle_error, target_rate, 0, 0, output, tuning_value); - } - } -#endif - // set targets for rate controller set_yaw_rate_target(target_rate, EARTH_FRAME); } @@ -513,17 +501,6 @@ get_rate_roll(int32_t target_rate) // constrain output output = constrain_int32(output, -5000, 5000); -#if LOGGING_ENABLED == ENABLED - // log output if PID logging is on and we are tuning the rate P, I or D gains - if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_RATE_ROLL_PITCH_KP || g.radio_tuning == CH6_RATE_ROLL_PITCH_KI || g.radio_tuning == CH6_RATE_ROLL_PITCH_KD) ) { - pid_log_counter++; // Note: get_rate_pitch pid logging relies on this function to update pid_log_counter so if you change the line above you must change the equivalent line in get_rate_pitch - if( pid_log_counter >= 10 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10 - pid_log_counter = 0; - Log_Write_PID(CH6_RATE_ROLL_PITCH_KP, rate_error, p, i, d, output, tuning_value); - } - } -#endif - // output control return output; } @@ -557,15 +534,6 @@ get_rate_pitch(int32_t target_rate) // constrain output output = constrain_int32(output, -5000, 5000); -#if LOGGING_ENABLED == ENABLED - // log output if PID logging is on and we are tuning the rate P, I or D gains - if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_RATE_ROLL_PITCH_KP || g.radio_tuning == CH6_RATE_ROLL_PITCH_KI || g.radio_tuning == CH6_RATE_ROLL_PITCH_KD) ) { - if( pid_log_counter == 0 ) { // relies on get_rate_roll having updated pid_log_counter - Log_Write_PID(CH6_RATE_ROLL_PITCH_KP+100, rate_error, p, i, d, output, tuning_value); - } - } -#endif - // output control return output; } @@ -597,17 +565,6 @@ get_rate_yaw(int32_t target_rate) output = p+i+d; output = constrain_int32(output, -4500, 4500); -#if LOGGING_ENABLED == ENABLED - // 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_RATE_KP ) { - pid_log_counter++; - if( pid_log_counter >= 10 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10 - pid_log_counter = 0; - Log_Write_PID(CH6_YAW_RATE_KP, rate_error, p, i, d, output, tuning_value); - } - } -#endif - // constrain output return output; } @@ -645,17 +602,6 @@ get_of_roll(int32_t input_roll) } // limit amount of change and maximum angle of_roll = constrain_int32(new_roll, (of_roll-20), (of_roll+20)); - - #if LOGGING_ENABLED == ENABLED - // log output if PID logging is on and we are tuning the rate P, I or D gains - if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_OPTFLOW_KP || g.radio_tuning == CH6_OPTFLOW_KI || g.radio_tuning == CH6_OPTFLOW_KD) ) { - pid_log_counter++; // Note: get_of_pitch pid logging relies on this function updating pid_log_counter so if you change the line above you must change the equivalent line in get_of_pitch - if( pid_log_counter >= 5 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10 - pid_log_counter = 0; - Log_Write_PID(CH6_OPTFLOW_KP, tot_x_cm, p, i, d, of_roll, tuning_value); - } - } - #endif // LOGGING_ENABLED == ENABLED } // limit max angle @@ -699,15 +645,6 @@ get_of_pitch(int32_t input_pitch) // limit amount of change of_pitch = constrain_int32(new_pitch, (of_pitch-20), (of_pitch+20)); - - #if LOGGING_ENABLED == ENABLED - // log output if PID logging is on and we are tuning the rate P, I or D gains - if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_OPTFLOW_KP || g.radio_tuning == CH6_OPTFLOW_KI || g.radio_tuning == CH6_OPTFLOW_KD) ) { - if( pid_log_counter == 0 ) { // relies on get_of_roll having updated the pid_log_counter - Log_Write_PID(CH6_OPTFLOW_KP+100, tot_y_cm, p, i, d, of_pitch, tuning_value); - } - } - #endif // LOGGING_ENABLED == ENABLED } // limit max angle @@ -919,17 +856,6 @@ get_throttle_accel(int16_t z_target_accel) output = constrain_float(p+i+d+g.throttle_cruise, g.throttle_min, g.throttle_max); -#if LOGGING_ENABLED == ENABLED - // log output if PID loggins is on and we are tuning the yaw - if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_THROTTLE_ACCEL_KP || g.radio_tuning == CH6_THROTTLE_ACCEL_KI || g.radio_tuning == CH6_THROTTLE_ACCEL_KD) ) { - pid_log_counter++; - if( pid_log_counter >= 10 ) { // (update rate / desired output rate) = (50hz / 10hz) = 5hz - pid_log_counter = 0; - Log_Write_PID(CH6_THROTTLE_ACCEL_KP, z_accel_error, p, i, d, output, tuning_value); - } - } -#endif - return output; } @@ -1059,17 +985,6 @@ get_throttle_rate(float z_target_speed) output += p; output = constrain_int32(output, -32000, 32000); -#if LOGGING_ENABLED == ENABLED - // log output if PID loggins is on and we are tuning the yaw - if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_THROTTLE_RATE_KP || g.radio_tuning == CH6_THROTTLE_RATE_KD) ) { - pid_log_counter++; - if( pid_log_counter >= 10 ) { // (update rate / desired output rate) = (50hz / 10hz) = 5hz - pid_log_counter = 0; - Log_Write_PID(CH6_THROTTLE_RATE_KP, z_rate_error, p, 0, 0, output, tuning_value); - } - } -#endif - // set target for accel based throttle controller set_throttle_accel_target(output); diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index c2bdfec360..49f4718ffb 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -46,7 +46,6 @@ print_log_menu(void) if (g.log_bitmask & MASK_LOG_CURRENT) cliSerial->printf_P(PSTR(" CURRENT")); if (g.log_bitmask & MASK_LOG_RCOUT) cliSerial->printf_P(PSTR(" RCOUT")); if (g.log_bitmask & MASK_LOG_OPTFLOW) cliSerial->printf_P(PSTR(" OPTFLOW")); - if (g.log_bitmask & MASK_LOG_PID) cliSerial->printf_P(PSTR(" PID")); if (g.log_bitmask & MASK_LOG_COMPASS) cliSerial->printf_P(PSTR(" COMPASS")); if (g.log_bitmask & MASK_LOG_INAV) cliSerial->printf_P(PSTR(" INAV")); if (g.log_bitmask & MASK_LOG_CAMERA) cliSerial->printf_P(PSTR(" CAMERA")); @@ -647,33 +646,6 @@ static void Log_Write_Data(uint8_t id, float value) } } -struct PACKED log_PID { - LOG_PACKET_HEADER; - uint8_t id; - int32_t error; - int32_t p; - int32_t i; - int32_t d; - int32_t output; - float gain; -}; - -// Write an PID packet -static void Log_Write_PID(uint8_t pid_id, int32_t error, int32_t p, int32_t i, int32_t d, int32_t output, float gain) -{ - struct log_PID pkt = { - LOG_PACKET_HEADER_INIT(LOG_PID_MSG), - id : pid_id, - error : error, - p : p, - i : i, - d : d, - output : output, - gain : gain - }; - DataFlash.WriteBlock(&pkt, sizeof(pkt)); -} - struct PACKED log_Camera { LOG_PACKET_HEADER; uint32_t gps_time; @@ -766,8 +738,6 @@ static const struct LogStructure log_structure[] PROGMEM = { "DU32", "BI", "Id,Value" }, { LOG_DATA_FLOAT_MSG, sizeof(log_Data_Float), "DFLT", "Bf", "Id,Value" }, - { LOG_PID_MSG, sizeof(log_PID), - "PID", "Biiiiif", "Id,Error,P,I,D,Out,Gain" }, { LOG_CAMERA_MSG, sizeof(log_Camera), "CAM", "IHLLeccC", "GPSTime,GPSWeek,Lat,Lng,Alt,Roll,Pitch,Yaw" }, { LOG_ERROR_MSG, sizeof(log_Error), @@ -840,7 +810,6 @@ static void Log_Write_Optflow() {} static void Log_Write_Nav_Tuning() {} static void Log_Write_Control_Tuning() {} static void Log_Write_Performance() {} -static void Log_Write_PID(uint8_t pid_id, int32_t error, int32_t p, int32_t i, int32_t d, int32_t output, float gain) {} static void Log_Write_Camera() {} static void Log_Write_Error(uint8_t sub_system, uint8_t error_code) {} static int8_t process_logs(uint8_t argc, const Menu::arg *argv) { diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 490fc01c74..773049b43b 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -250,7 +250,7 @@ #define LOG_STARTUP_MSG 0x0A #define LOG_OPTFLOW_MSG 0x0C #define LOG_EVENT_MSG 0x0D -#define LOG_PID_MSG 0x0E +#define LOG_PID_MSG 0x0E // deprecated #define LOG_COMPASS_MSG 0x0F #define LOG_INAV_MSG 0x11 #define LOG_CAMERA_MSG 0x12 @@ -278,7 +278,7 @@ #define MASK_LOG_CURRENT (1<<9) #define MASK_LOG_RCOUT (1<<10) #define MASK_LOG_OPTFLOW (1<<11) -#define MASK_LOG_PID (1<<12) +#define MASK_LOG_PID (1<<12) // deprecated #define MASK_LOG_COMPASS (1<<13) #define MASK_LOG_INAV (1<<14) #define MASK_LOG_CAMERA (1<<15) diff --git a/ArduCopter/heli.pde b/ArduCopter/heli.pde index 5e1732faaf..9d0676a3b5 100644 --- a/ArduCopter/heli.pde +++ b/ArduCopter/heli.pde @@ -279,17 +279,6 @@ get_heli_rate_yaw(int32_t target_rate) pid_saturated = false; // unfreeze integrator } -#if LOGGING_ENABLED == ENABLED - // 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_RATE_KP || g.radio_tuning == CH6_YAW_RATE_KD) ) { - pid_log_counter++; - if( pid_log_counter >= 10 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10 - pid_log_counter = 0; - Log_Write_PID(CH6_YAW_RATE_KP, rate_error, p, i, d, output, tuning_value); - } - } -#endif - return output; // output control } @@ -344,4 +333,4 @@ static void heli_update_rotor_speed_targets() } } -#endif // FRAME_CONFIG == HELI_FRAME \ No newline at end of file +#endif // FRAME_CONFIG == HELI_FRAME