From 1792710f41ab04aaafdca340a13e41f165d3db4b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 27 May 2015 14:32:05 +1000 Subject: [PATCH] Copter: added a bit in LOG_BITMASK for PID logging --- ArduCopter/ArduCopter.pde | 20 ++++++++++++-------- ArduCopter/Log.pde | 2 ++ ArduCopter/Parameters.h | 2 +- ArduCopter/Parameters.pde | 2 +- ArduCopter/defines.h | 2 +- 5 files changed, 17 insertions(+), 11 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 1725969d79..53c81f7483 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -972,10 +972,12 @@ static void ten_hz_logging_loop() if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) { Log_Write_Attitude(); Log_Write_Rate(); - DataFlash.Log_Write_PID(LOG_PIDR_MSG, g.pid_rate_roll.get_pid_info() ); - DataFlash.Log_Write_PID(LOG_PIDP_MSG, g.pid_rate_pitch.get_pid_info() ); - DataFlash.Log_Write_PID(LOG_PIDY_MSG, g.pid_rate_yaw.get_pid_info() ); - DataFlash.Log_Write_PID(LOG_PIDA_MSG, g.pid_accel_z.get_pid_info() ); + if (should_log(MASK_LOG_PID)) { + DataFlash.Log_Write_PID(LOG_PIDR_MSG, g.pid_rate_roll.get_pid_info() ); + DataFlash.Log_Write_PID(LOG_PIDP_MSG, g.pid_rate_pitch.get_pid_info() ); + DataFlash.Log_Write_PID(LOG_PIDY_MSG, g.pid_rate_yaw.get_pid_info() ); + DataFlash.Log_Write_PID(LOG_PIDA_MSG, g.pid_accel_z.get_pid_info() ); + } } if (should_log(MASK_LOG_MOTBATT)) { Log_Write_MotBatt(); @@ -1004,10 +1006,12 @@ static void fifty_hz_logging_loop() if (should_log(MASK_LOG_ATTITUDE_FAST)) { Log_Write_Attitude(); Log_Write_Rate(); - DataFlash.Log_Write_PID(LOG_PIDR_MSG, g.pid_rate_roll.get_pid_info() ); - DataFlash.Log_Write_PID(LOG_PIDP_MSG, g.pid_rate_pitch.get_pid_info() ); - DataFlash.Log_Write_PID(LOG_PIDY_MSG, g.pid_rate_yaw.get_pid_info() ); - DataFlash.Log_Write_PID(LOG_PIDA_MSG, g.pid_accel_z.get_pid_info() ); + if (should_log(MASK_LOG_PID)) { + DataFlash.Log_Write_PID(LOG_PIDR_MSG, g.pid_rate_roll.get_pid_info() ); + DataFlash.Log_Write_PID(LOG_PIDP_MSG, g.pid_rate_pitch.get_pid_info() ); + DataFlash.Log_Write_PID(LOG_PIDY_MSG, g.pid_rate_yaw.get_pid_info() ); + DataFlash.Log_Write_PID(LOG_PIDA_MSG, g.pid_accel_z.get_pid_info() ); + } } // log IMU data if we're not already logging at the higher rate diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index d01d57cc68..7ee15686bd 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -49,6 +49,7 @@ print_log_menu(void) if (g.log_bitmask & MASK_LOG_OPTFLOW) cliSerial->printf_P(PSTR(" OPTFLOW")); if (g.log_bitmask & MASK_LOG_COMPASS) cliSerial->printf_P(PSTR(" COMPASS")); if (g.log_bitmask & MASK_LOG_CAMERA) cliSerial->printf_P(PSTR(" CAMERA")); + if (g.log_bitmask & MASK_LOG_PID) cliSerial->printf_P(PSTR(" PID")); } cliSerial->println(); @@ -134,6 +135,7 @@ select_logs(uint8_t argc, const Menu::arg *argv) TARG(OPTFLOW); TARG(COMPASS); TARG(CAMERA); + TARG(PID); #undef TARG } diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 62872fc455..9d33aa272b 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -415,7 +415,7 @@ public: AP_Int8 land_repositioning; AP_Float ekfcheck_thresh; - AP_Int8 gcs_pid_mask; + AP_Int16 gcs_pid_mask; #if FRAME_CONFIG == HELI_FRAME // Heli diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 36ec914526..b4fa187ab1 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -344,7 +344,7 @@ const AP_Param::Info var_info[] PROGMEM = { // @Param: LOG_BITMASK // @DisplayName: Log bitmask // @Description: 4 byte bitmap of log types to enable - // @Values: 830:Default,894:Default+RCIN,958:Default+IMU,1854:Default+Motors,-6146:NearlyAll-AC315,45054:NearlyAll,131070:All+DisarmedLogging,131071:All+FastATT,262142:All+MotBatt,393214:All+FastIMU,0:Disabled + // @Values: 830:Default,894:Default+RCIN,958:Default+IMU,1854:Default+Motors,-6146:NearlyAll-AC315,45054:NearlyAll,131070:All+DisarmedLogging,131071:All+FastATT,262142:All+MotBatt,393214:All+FastIMU,All+FastIMU+PID:397310,0:Disabled // @User: Standard GSCALAR(log_bitmask, "LOG_BITMASK", DEFAULT_LOG_BITMASK), diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 28efba950e..3ed0adf6a7 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -252,7 +252,7 @@ enum FlipState { #define MASK_LOG_CURRENT (1<<9) #define MASK_LOG_RCOUT (1<<10) #define MASK_LOG_OPTFLOW (1<<11) -#define MASK_LOG_PID (1<<12) // deprecated +#define MASK_LOG_PID (1<<12) #define MASK_LOG_COMPASS (1<<13) #define MASK_LOG_INAV (1<<14) // deprecated #define MASK_LOG_CAMERA (1<<15)