diff options
| author | Roman Bapst <romanbapst@yahoo.de> | 2015-04-13 11:23:27 +0200 |
|---|---|---|
| committer | Roman Bapst <romanbapst@yahoo.de> | 2015-04-13 12:30:49 +0200 |
| commit | c7653c2b3a128ea208d088a0d29f394f3a525727 (patch) | |
| tree | b53c9531958ced967e2f62c976e13a32738036f7 | |
| parent | 998910dd8f5d00354a9f0238e60fb00bb5166031 (diff) | |
| download | px4-firmware-anti_windup.tar.gz px4-firmware-anti_windup.tar.bz2 px4-firmware-anti_windup.zip | |
log multirotor attitude controller statusanti_windup
| -rw-r--r-- | src/modules/sdlog2/sdlog2.c | 14 | ||||
| -rw-r--r-- | src/modules/sdlog2/sdlog2_messages.h | 9 |
2 files changed, 23 insertions, 0 deletions
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 0bfd356a1..b09ba72a9 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -99,6 +99,7 @@ #include <uORB/topics/wind_estimate.h> #include <uORB/topics/encoders.h> #include <uORB/topics/vtol_vehicle_status.h> +#include <uORB/topics/mc_att_ctrl_status.h> #include <systemlib/systemlib.h> #include <systemlib/param/param.h> @@ -1027,6 +1028,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct wind_estimate_s wind_estimate; struct encoders_s encoders; struct vtol_vehicle_status_s vtol_status; + struct mc_att_ctrl_status_s mc_att_ctrl_status; } buf; memset(&buf, 0, sizeof(buf)); @@ -1071,6 +1073,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_TECS_s log_TECS; struct log_WIND_s log_WIND; struct log_ENCD_s log_ENCD; + struct log_MACS_s log_MACS; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1111,6 +1114,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int servorail_status_sub; int wind_sub; int encoders_sub; + int mc_att_ctrl_status_sub; } subs; subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -1142,6 +1146,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.system_power_sub = orb_subscribe(ORB_ID(system_power)); subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status)); subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate)); + 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); @@ -1821,6 +1826,15 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(ENCD); } + /* --- 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 */ if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { /* only request write if several packets can be written at once */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index a1fe2c95d..2258327b0 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -449,6 +449,14 @@ struct log_VTOL_s { float airspeed_tot; }; +/* --- 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 **********/ /* --- TIME - TIME STAMP --- */ @@ -517,6 +525,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), LOG_FORMAT(ENCD, "qfqf", "cnt0,vel0,cnt1,vel1"), + LOG_FORMAT(MACS, "fff", "RRint,PRint,YRint"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ |