From 00de10d7969d380518f084a655bb0872cb3d746e Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 4 Apr 2022 15:54:12 +1000 Subject: [PATCH] AP_HAL_ChibiOS: use structure for writing out WDOG,MON message --- libraries/AP_HAL_ChibiOS/LogStructure.h | 77 +++++++++++++++++++++++++ libraries/AP_HAL_ChibiOS/Scheduler.cpp | 65 +++++++++++---------- 2 files changed, 113 insertions(+), 29 deletions(-) create mode 100644 libraries/AP_HAL_ChibiOS/LogStructure.h diff --git a/libraries/AP_HAL_ChibiOS/LogStructure.h b/libraries/AP_HAL_ChibiOS/LogStructure.h new file mode 100644 index 0000000000..236f9bac78 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/LogStructure.h @@ -0,0 +1,77 @@ +#pragma once + +#include + +#define LOG_IDS_FROM_HAL_CHIBIOS \ + LOG_MON_MSG, \ + LOG_WDOG_MSG + +// @LoggerMessage: MON +// @Description: Main loop performance monitoring message. +// @Field: TimeUS: Time since system startup +// @Field: Dly: Loop delay so far +// @Field: Tsk: Current task +// @Field: IErr: Internal error mask +// @Field: IErrCnt: Count of internal error occurances +// @Field: IErrLn: Internal Error line +// @Field: MM: MAVLink message currently being processed +// @Field: MC: MAVLink command currently being processed +// @Field: SmLn: If semaphore taken, line of semaphore take call +// @Field: SPICnt: count of SPI transactions +// @Field: I2CCnt: count of i2c transactions +struct PACKED log_MON { + LOG_PACKET_HEADER; + uint64_t time_us; + uint32_t loop_delay; + int8_t current_task; + uint32_t internal_error_mask; + uint16_t internal_error_count; + uint16_t internal_error_line; + uint16_t mavmsg; + uint16_t mavcmd; + uint16_t semline; + uint32_t spicnt; + uint32_t i2ccnt; +}; + +// @LoggerMessage: WDOG +// @Description: Watchdog diagnostics +// @Field: TimeUS: Time since system startup +// @Field: Tsk: current task number +// @Field: IE: internal error mast +// @Field: IEC: internal error count +// @Field: IEL: line internal error was raised on +// @Field: MvMsg: mavlink message being acted on +// @Field: MvCmd: mavlink command being acted on +// @Field: SmLn: line semaphore was taken on +// @Field: FL: fault_line +// @Field: FT: fault_type +// @Field: FA: fault address +// @Field: FP: fault thread priority +// @Field: ICSR: ICS regiuster +// @Field: LR: long return address +// @Field: TN: Thread name +struct PACKED log_WDOG { + LOG_PACKET_HEADER; + uint64_t time_us; + int8_t scheduler_task; + uint32_t internal_errors; + uint16_t internal_error_count; + uint16_t internal_error_last_line; + uint16_t last_mavlink_msgid; + uint16_t last_mavlink_cmd; + uint16_t semaphore_line; + uint16_t fault_line; + uint16_t fault_type; + uint32_t fault_addr; + uint8_t fault_thd_prio; + uint32_t fault_icsr; + uint32_t fault_lr; + char thread_name4[4]; +}; + +#define LOG_STRUCTURE_FROM_HAL_CHIBIOS \ + { LOG_MON_MSG, sizeof(log_MON), \ + "MON","QIbIHHHHHII","TimeUS,Dly,Tsk,IErr,IErrCnt,IErrLn,MM,MC,SmLn,SPICnt,I2CCnt", "s----------", "F----------", false }, \ + { LOG_WDOG_MSG, sizeof(log_WDOG), \ + "WDOG","QbIHHHHHHHIBIIn","TimeUS,Tsk,IE,IEC,IEL,MvMsg,MvCmd,SmLn,FL,FT,FA,FP,ICSR,LR,TN", "s--------------", "F--------------", false }, diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.cpp b/libraries/AP_HAL_ChibiOS/Scheduler.cpp index c8a566c609..66c8bb762b 100644 --- a/libraries/AP_HAL_ChibiOS/Scheduler.cpp +++ b/libraries/AP_HAL_ChibiOS/Scheduler.cpp @@ -410,19 +410,22 @@ void Scheduler::_monitor_thread(void *arg) #if HAL_LOGGING_ENABLED const AP_HAL::Util::PersistentData &pd = hal.util->persistent_data; if (AP_Logger::get_singleton()) { - AP::logger().Write("MON", "TimeUS,LDelay,Task,IErr,IErrCnt,IErrLn,MavMsg,MavCmd,SemLine,SPICnt,I2CCnt", "QIbIHHHHHII", - AP_HAL::micros64(), - loop_delay, - pd.scheduler_task, - pd.internal_errors, - pd.internal_error_count, - pd.internal_error_last_line, - pd.last_mavlink_msgid, - pd.last_mavlink_cmd, - pd.semaphore_line, - pd.spi_count, - pd.i2c_count); - } + const struct log_MON mon{ + LOG_PACKET_HEADER_INIT(LOG_MON_MSG), + time_us : AP_HAL::micros64(), + loop_delay : loop_delay, + current_task : pd.scheduler_task, + internal_error_mask : pd.internal_errors, + internal_error_count : pd.internal_error_count, + internal_error_line : pd.internal_error_last_line, + mavmsg : pd.last_mavlink_msgid, + mavcmd : pd.last_mavlink_cmd, + semline : pd.semaphore_line, + spicnt : pd.spi_count, + i2ccnt : pd.i2c_count + }; + AP::logger().WriteCriticalBlock(&mon, sizeof(mon)); + } #endif } if (loop_delay >= 500 && !sched->in_expected_delay()) { @@ -435,22 +438,26 @@ void Scheduler::_monitor_thread(void *arg) log_wd_counter = 0; // log watchdog message once a second const AP_HAL::Util::PersistentData &pd = hal.util->last_persistent_data; - AP::logger().WriteCritical("WDOG", "TimeUS,Tsk,IE,IEC,IEL,MvMsg,MvCmd,SmLn,FL,FT,FA,FP,ICSR,LR,TN", "QbIHHHHHHHIBIIn", - AP_HAL::micros64(), - pd.scheduler_task, - pd.internal_errors, - pd.internal_error_count, - pd.internal_error_last_line, - pd.last_mavlink_msgid, - pd.last_mavlink_cmd, - pd.semaphore_line, - pd.fault_line, - pd.fault_type, - pd.fault_addr, - pd.fault_thd_prio, - pd.fault_icsr, - pd.fault_lr, - pd.thread_name4); + struct log_WDOG wdog{ + LOG_PACKET_HEADER_INIT(LOG_WDOG_MSG), + time_us : AP_HAL::micros64(), + scheduler_task : pd.scheduler_task, + internal_errors : pd.internal_errors, + internal_error_count : pd.internal_error_count, + internal_error_last_line : pd.internal_error_last_line, + last_mavlink_msgid : pd.last_mavlink_msgid, + last_mavlink_cmd : pd.last_mavlink_cmd, + semaphore_line : pd.semaphore_line, + fault_line : pd.fault_line, + fault_type : pd.fault_type, + fault_addr : pd.fault_addr, + fault_thd_prio : pd.fault_thd_prio, + fault_icsr : pd.fault_icsr, + fault_lr : pd.fault_lr + }; + memcpy(wdog.thread_name4, pd.thread_name4, ARRAY_SIZE(wdog.thread_name4)); + + AP::logger().WriteCriticalBlock(&wdog, sizeof(wdog)); } #endif // HAL_LOGGING_ENABLED