From e097affd7aea14ffed53d31bdbee0576ae619dd5 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 13 Apr 2015 11:23:27 +0200 Subject: log multirotor attitude controller status --- src/modules/sdlog2/sdlog2.c | 13 +++++++++++++ src/modules/sdlog2/sdlog2_messages.h | 8 ++++++++ 2 files changed, 21 insertions(+) (limited to 'src') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index ed93a06b1..1b6d34d70 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 598f12a53..1f97cf722 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 */ -- cgit v1.2.3