From bfc373aeff23c7f2cbd933ddfb0808ce64b4c8a1 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 16 Nov 2017 14:20:40 +1100 Subject: [PATCH] Copter: move logging of PM messages to AP_Scheduler --- ArduCopter/ArduCopter.cpp | 7 +------ ArduCopter/Copter.cpp | 1 - ArduCopter/Copter.h | 3 --- ArduCopter/GCS_Mavlink.cpp | 1 - ArduCopter/Log.cpp | 33 --------------------------------- ArduCopter/defines.h | 1 - 6 files changed, 1 insertion(+), 45 deletions(-) diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index b178fa87d2..f8e8ae0f06 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -192,12 +192,7 @@ void Copter::setup() void Copter::perf_update(void) { - if (should_log(MASK_LOG_PM)) { - Log_Write_Performance(); - } - scheduler.update_logging(); - scheduler.perf_info.reset(); - pmTest1 = 0; + scheduler.update_logging(should_log(MASK_LOG_PM)); } void Copter::loop() diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 78cca09a21..993d35304f 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -46,7 +46,6 @@ Copter::Copter(void) yaw_look_at_heading_slew(0), yaw_look_ahead_bearing(0.0f), inertial_nav(ahrs), - pmTest1(0), auto_trim_counter(0), in_mavlink_delay(false), param_loader(var_info), diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 7b4b659071..71a36799b8 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -468,9 +468,6 @@ private: AC_WPNav *wp_nav; AC_Circle *circle_nav; - // Performance monitoring - int16_t pmTest1; - // System Timers // -------------- // arm_time_ms - Records when vehicle was armed. Will be Zero if we are disarmed. diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 59a3670f1c..03d0a51cec 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -720,7 +720,6 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes if(msg->sysid != copter.g.sysid_my_gcs) break; copter.failsafe.last_heartbeat_ms = AP_HAL::millis(); - copter.pmTest1++; break; } diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index 4bd3c19f2b..268201d9c6 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -180,37 +180,6 @@ void Copter::Log_Write_Control_Tuning() DataFlash.WriteBlock(&pkt, sizeof(pkt)); } -struct PACKED log_Performance { - LOG_PACKET_HEADER; - uint64_t time_us; - uint16_t num_long_running; - uint16_t num_loops; - uint32_t max_time; - int16_t pm_test; - uint8_t i2c_lockup_count; - uint16_t ins_error_count; - uint32_t log_dropped; - uint32_t mem_avail; -}; - -// Write a performance monitoring packet -void Copter::Log_Write_Performance() -{ - struct log_Performance pkt = { - LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG), - time_us : AP_HAL::micros64(), - num_long_running : scheduler.perf_info.get_num_long_running(), - num_loops : scheduler.perf_info.get_num_loops(), - max_time : scheduler.perf_info.get_max_time(), - pm_test : pmTest1, - i2c_lockup_count : 0, - ins_error_count : ins.error_count(), - log_dropped : DataFlash.num_dropped() - scheduler.perf_info.get_num_dropped(), - hal.util->available_memory() - }; - DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt)); -} - // Write an attitude packet void Copter::Log_Write_Attitude() { @@ -639,8 +608,6 @@ const struct LogStructure Copter::log_structure[] = { "NTUN", "Qffffffffff", "TimeUS,DPosX,DPosY,PosX,PosY,DVelX,DVelY,VelX,VelY,DAccX,DAccY", "smmmmnnnnoo", "FBBBBBBBBBB" }, { LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning), "CTUN", "Qffffffeccfhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt", "s----mmmmmmnn", "F----00BBBBBB" }, - { LOG_PERFORMANCE_MSG, sizeof(log_Performance), - "PM", "QHHIhBHII", "TimeUS,NLon,NLoop,MaxT,PMT,I2CErr,INSErr,LogDrop,Mem", "s-------b", "F-------0" }, { LOG_MOTBATT_MSG, sizeof(log_MotBatt), "MOTB", "Qffff", "TimeUS,LiftMax,BatVolt,BatRes,ThLimit", "s-vw-", "F-00-" }, { LOG_EVENT_MSG, sizeof(log_Event), diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index df1ab16f61..a0927071a6 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -315,7 +315,6 @@ enum DevOptions { #define TYPE_GROUNDSTART_MSG 0x01 #define LOG_CONTROL_TUNING_MSG 0x04 #define LOG_NAV_TUNING_MSG 0x05 -#define LOG_PERFORMANCE_MSG 0x06 #define LOG_OPTFLOW_MSG 0x0C #define LOG_EVENT_MSG 0x0D #define LOG_PID_MSG 0x0E // deprecated