From e097affd7aea14ffed53d31bdbee0576ae619dd5 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 13 Apr 2015 11:23:27 +0200 Subject: [PATCH] log multirotor attitude controller status --- nuttx-configs/px4-stm32f4discovery/nsh/defconfig | 2 +- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- src/modules/sdlog2/sdlog2.c | 13 +++++++++++++ src/modules/sdlog2/sdlog2_messages.h | 8 ++++++++ 5 files changed, 24 insertions(+), 3 deletions(-) diff --git a/nuttx-configs/px4-stm32f4discovery/nsh/defconfig b/nuttx-configs/px4-stm32f4discovery/nsh/defconfig index 8398c5380907..6a2470bea975 100644 --- a/nuttx-configs/px4-stm32f4discovery/nsh/defconfig +++ b/nuttx-configs/px4-stm32f4discovery/nsh/defconfig @@ -392,7 +392,7 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=32 +CONFIG_NFILE_DESCRIPTORS=44 CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index cd410051c75d..389cb5ab5b69 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -405,7 +405,7 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=42 +CONFIG_NFILE_DESCRIPTORS=44 CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 4ccc5dacb69f..d331aa26a73d 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -439,7 +439,7 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=42 +CONFIG_NFILE_DESCRIPTORS=44 CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index ed93a06b127b..1b6d34d70b46 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -100,6 +100,7 @@ #include #include #include +#include #include #include @@ -1029,6 +1030,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct encoders_s encoders; struct vtol_vehicle_status_s vtol_status; struct time_offset_s time_offset; + struct mc_att_ctrl_status_s mc_att_ctrl_status; } buf; memset(&buf, 0, sizeof(buf)); @@ -1074,6 +1076,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_WIND_s log_WIND; struct log_ENCD_s log_ENCD; struct log_TSYN_s log_TSYN; + struct log_MACS_s log_MACS; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1115,6 +1118,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int wind_sub; int encoders_sub; int tsync_sub; + int mc_att_ctrl_status_sub; } subs; subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -1147,6 +1151,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status)); subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate)); subs.tsync_sub = orb_subscribe(ORB_ID(time_offset)); + subs.mc_att_ctrl_status_sub = orb_subscribe(ORB_ID(mc_att_ctrl_status)); /* we need to rate-limit wind, as we do not need the full update rate */ orb_set_interval(subs.wind_sub, 90); @@ -1831,6 +1836,14 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_TSYN_MSG; log_msg.body.log_TSYN.time_offset = buf.time_offset.offset_ns; LOGBUFFER_WRITE_AND_COUNT(TSYN); + + /* --- MULTIROTOR ATTITUDE CONTROLLER STATUS --- */ + if (copy_if_updated(ORB_ID(mc_att_ctrl_status), subs.mc_att_ctrl_status_sub, &buf.mc_att_ctrl_status)) { + log_msg.msg_type = LOG_MACS_MSG; + log_msg.body.log_MACS.roll_rate_integ = buf.mc_att_ctrl_status.roll_rate_integ; + log_msg.body.log_MACS.pitch_rate_integ = buf.mc_att_ctrl_status.pitch_rate_integ; + log_msg.body.log_MACS.yaw_rate_integ = buf.mc_att_ctrl_status.yaw_rate_integ; + LOGBUFFER_WRITE_AND_COUNT(MACS); } /* signal the other thread new data, but not yet unlock */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 598f12a53623..1f97cf7222fb 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -453,6 +453,13 @@ struct log_VTOL_s { #define LOG_TSYN_MSG 43 struct log_TSYN_s { uint64_t time_offset; + +/* --- MACS - MULTIROTOR ATTITUDE CONTROLLER STATUS */ +#define LOG_MACS_MSG 42 +struct log_MACS_s { + float roll_rate_integ; + float pitch_rate_integ; + float yaw_rate_integ; }; /********** SYSTEM MESSAGES, ID > 0x80 **********/ @@ -524,6 +531,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), LOG_FORMAT(ENCD, "qfqf", "cnt0,vel0,cnt1,vel1"), LOG_FORMAT(TSYN, "Q", "TimeOffset"), + LOG_FORMAT(MACS, "fff", "RRint,PRint,YRint"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */