diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 8dc3a76ecc..4be40211aa 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1211,6 +1211,8 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GPS_s log_GPS; struct log_ATTC_s log_ATTC; struct log_STAT_s log_STAT; + struct log_LOAD_s log_LOAD; + struct log_COMM_s log_COMM; struct log_VTOL_s log_VTOL; struct log_RC_s log_RC; struct log_OUT_s log_OUT; @@ -1515,6 +1517,24 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(STAT); } + /* --- COMMANDER INTERNAL STATE --- */ + bool commander_state_updated = copy_if_updated(ORB_ID(commander_state), &subs.commander_state_sub, + &buf.commander_state); + + if (commander_state_updated) { + log_msg.msg_type = LOG_COMM_MSG; + log_msg.body.log_COMM.main_state = buf.commander_state.main_state; + LOGBUFFER_WRITE_AND_COUNT(COMM); + } + + + bool cpuload_updated = copy_if_updated(ORB_ID(cpuload), &subs.cpuload_sub, &buf.cpuload); + if (cpuload_updated) { + log_msg.msg_type = LOG_LOAD_MSG; + log_msg.body.log_LOAD.cpu_load = buf.cpuload.load; + LOGBUFFER_WRITE_AND_COUNT(LOAD); + } + /* --- EKF2 REPLAY --- */ if (log_type == LOG_TYPE_ALL || log_type == LOG_TYPE_REPLAY_ONLY) { diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index c5dbffe6f3..e0c934e629 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -177,11 +177,9 @@ struct log_ATTC_s { /* --- STAT - VEHICLE STATE --- */ #define LOG_STAT_MSG 10 struct log_STAT_s { - uint8_t main_state; uint8_t nav_state; uint8_t arming_state; uint8_t failsafe; - float load; }; /* --- RC - RC INPUT CHANNELS --- */ @@ -597,11 +595,24 @@ struct log_CAMT_s { uint32_t seq; }; +/* --- LAND DETECTOR --- */ #define LOG_LAND_MSG 57 struct log_LAND_s { uint8_t landed; }; +/* --- SYSTEM LOAD --- */ +#define LOG_LOAD_MSG 58 +struct log_LOAD_s { + float cpu_load; +}; + +/* --- COMMANDER INTERNAL STATE --- */ +#define LOG_COMM_MSG 59 +struct log_COMM_s { + uint8_t main_state; +}; + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -643,7 +654,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"), LOG_FORMAT_S(ATTC, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT_S(ATC1, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), - LOG_FORMAT(STAT, "BBBBf", "MainState,NavState,ArmS,Failsafe,Load"), + LOG_FORMAT(STAT, "BBB", "NavState,ArmS,Failsafe"), LOG_FORMAT(VTOL, "fBBB", "Arsp,RwMode,TransMode,Failsafe"), LOG_FORMAT(CTS, "fffffff", "Vx_b,Vy_b,Vz_b,Vinf,P,Q,R"), LOG_FORMAT(RC, "ffffffffffffBBBL", "C0,C1,C2,C3,C4,C5,C6,C7,C8,C9,C10,C11,RSSI,CNT,Lost,Drop"), @@ -688,6 +699,8 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(RPL4, "Qf", "Trng,rng"), LOG_FORMAT(RPL6, "Qfffff", "Tasp,inAsp,trAsp,ufAsp,tpAsp,confAsp"), LOG_FORMAT(LAND, "B", "Landed"), + LOG_FORMAT(LOAD, "f", "CPU"), + LOG_FORMAT(COMM, "B", "MainState"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ LOG_FORMAT(TIME, "Q", "StartTime"),