fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

log multirotor attitude controller status

Browse Source
This commit is contained in:
Roman Bapst 2015-04-13 11:23:27 +02:00 committed by Lorenz Meier
parent b52d0ed8a2
commit e097affd7a
5 changed files with 24 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
nuttx-configs/px4-stm32f4discovery/nsh/defconfig
View File

@ -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

2
nuttx-configs/px4fmu-v1/nsh/defconfig
View File

@ -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

2
nuttx-configs/px4fmu-v2/nsh/defconfig
View File

@ -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

13
src/modules/sdlog2/sdlog2.c
View File

@ -100,6 +100,7 @@
#include <uORB/topics/encoders.h>
#include <uORB/topics/vtol_vehicle_status.h>
#include <uORB/topics/time_offset.h>
#include <uORB/topics/mc_att_ctrl_status.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
@ -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 */

8
src/modules/sdlog2/sdlog2_messages.h
View File

@ -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 */