fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 11:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

log ekf2 innovations and innovation variances

Browse Source
This commit is contained in:
Roman
2016-01-22 14:24:36 +01:00
committed by tumbili
parent 21f7641e8d
commit c185a12c8e
3 changed files with 65 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sdlog2/sdlog2.c
+29
View File
@@ -109,6 +109,7 @@
#include <uORB/topics/vtol_vehicle_status.h>
#include <uORB/topics/time_offset.h>
#include <uORB/topics/mc_att_ctrl_status.h>
#include <uORB/topics/ekf2_innovations.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
@@ -1095,6 +1096,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct time_offset_s time_offset;
struct mc_att_ctrl_status_s mc_att_ctrl_status;
struct control_state_s ctrl_state;
struct ekf2_innovations_s innovations;
} buf;
memset(&buf, 0, sizeof(buf));
@@ -1144,6 +1146,8 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_TSYN_s log_TSYN;
struct log_MACS_s log_MACS;
struct log_CTS_s log_CTS;
struct log_EST4_s log_INO1;
struct log_EST5_s log_INO2;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
@@ -1187,6 +1191,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int tsync_sub;
int mc_att_ctrl_status_sub;
int ctrl_state_sub;
int innov_sub;
} subs;
subs.cmd_sub = -1;
@@ -1222,6 +1227,7 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.mc_att_ctrl_status_sub = -1;
subs.ctrl_state_sub = -1;
subs.encoders_sub = -1;
subs.innov_sub = -1;
/* add new topics HERE */
@@ -1810,6 +1816,29 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(EST3);
}
/* --- EKF2 INNOVATIONS --- */
if (copy_if_updated(ORB_ID(ekf2_innovations), &subs.innov_sub, &buf.innovations)) {
log_msg.msg_type = LOG_EST4_MSG;
memset(&(log_msg.body.log_INO1.s), 0, sizeof(log_msg.body.log_INO1.s));
for (unsigned i = 0; i < 6; i++) {
log_msg.body.log_INO1.s[i] = buf.innovations.vel_pos_innov[i];
log_msg.body.log_INO1.s[i + 6] = buf.innovations.vel_pos_innov_var[i];
}
LOGBUFFER_WRITE_AND_COUNT(EST4);
log_msg.msg_type = LOG_EST5_MSG;
memset(&(log_msg.body.log_INO2.s), 0, sizeof(log_msg.body.log_INO2.s));
for (unsigned i = 0; i < 3; i++) {
log_msg.body.log_INO2.s[i] = buf.innovations.mag_innov[i];
log_msg.body.log_INO2.s[i + 3] = buf.innovations.mag_innov_var[i];
}
log_msg.body.log_INO2.s[6] = buf.innovations.heading_innov;
log_msg.body.log_INO2.s[7] = buf.innovations.heading_innov_var;
LOGBUFFER_WRITE_AND_COUNT(EST5);
}
/* --- TECS STATUS --- */
if (copy_if_updated(ORB_ID(tecs_status), &subs.tecs_status_sub, &buf.tecs_status)) {
log_msg.msg_type = LOG_TECS_MSG;