yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

sdlog: setup flow innovation logging

Browse Source
This commit is contained in:
bugobliterator
2016-02-23 13:07:40 -08:00
committed by Lorenz Meier
parent 47f79a66d1
commit 01ee608117
4 changed files with 21 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
msg/ekf2_innovations.msg
View File

@@ -2,6 +2,8 @@ uint64 timestamp # Timestamp in microseconds since boot
float32[6] vel_pos_innov # velocity and position innovations
float32[3] mag_innov # earth magnetic field innovations
float32 heading_innov # heading innovation
float32[2] flow_innov # flow innovation
float32[6] vel_pos_innov_var # velocity and position innovation variances
float32[3] mag_innov_var # earth magnetic field innovation variance
float32 heading_innov_var # heading innovation variance
float32[2] flow_innov_var # flow innovation variance

3
src/modules/ekf2/ekf2_main.cpp
View File

@@ -612,11 +612,12 @@ void Ekf2::task_main()
_ekf->get_vel_pos_innov(&innovations.vel_pos_innov[0]);
_ekf->get_mag_innov(&innovations.mag_innov[0]);
_ekf->get_heading_innov(&innovations.heading_innov);
_ekf->get_flow_innov(&innovations.flow_innov[0]);
_ekf->get_vel_pos_innov_var(&innovations.vel_pos_innov_var[0]);
_ekf->get_mag_innov_var(&innovations.mag_innov_var[0]);
_ekf->get_heading_innov_var(&innovations.heading_innov_var);
_ekf->get_flow_innov_var(&innovations.flow_innov_var[0]);
if (_estimator_innovations_pub == nullptr) {
_estimator_innovations_pub = orb_advertise(ORB_ID(ekf2_innovations), &innovations);

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

@@ -1200,6 +1200,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_CAMT_s log_CAMT;
struct log_RPL1_s log_RPL1;
struct log_RPL2_s log_RPL2;
struct log_EST6_s log_INO3;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
@@ -1858,6 +1859,7 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_AIRS.air_temperature_celsius = buf.airspeed.air_temperature_celsius;
LOGBUFFER_WRITE_AND_COUNT(AIRS);
}
<<<<<<< HEAD
/* --- ESCs --- */
if (copy_if_updated(ORB_ID(esc_status), &subs.esc_sub, &buf.esc)) {
@@ -1995,6 +1997,13 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_INO2.s[7] = buf.innovations.heading_innov_var;
LOGBUFFER_WRITE_AND_COUNT(EST5);
log_msg.msg_type = LOG_EST6_MSG;
memset(&(log_msg.body.log_INO3.s), 0, sizeof(log_msg.body.log_INO3.s));
for(unsigned i = 0; i < 2; i++) {
log_msg.body.log_INO3.s[i] = buf.innovations.flow_innov[i];
log_msg.body.log_INO3.s[i + 2] = buf.innovations.flow_innov_var[i];
}
LOGBUFFER_WRITE_AND_COUNT(EST6);
}
/* --- TECS STATUS --- */

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

@@ -423,12 +423,18 @@ struct log_EST4_s {
float s[12];
};
/* --- EST4 - ESTIMATOR INNOVATIONS --- */
/* --- EST5 - ESTIMATOR INNOVATIONS --- */
#define LOG_EST5_MSG 49
struct log_EST5_s {
float s[8];
};
/* --- EST6 - ESTIMATOR INNOVATIONS --- */
#define LOG_EST6_MSG 51
struct log_EST6_s {
float s[4];
};
/* --- TEL0..3 - TELEMETRY STATUS --- */
#define LOG_TEL0_MSG 36
#define LOG_TEL1_MSG 37
@@ -621,6 +627,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(EST3, "ffffffffffffffff", "P12,P13,P14,P15,P16,P17,P18,P19,P20,P21,P22,P23,P24,P25,P26,P27"),
LOG_FORMAT(EST4, "ffffffffffff", "VxI,VyI,VzI,PxI,PyI,PzI,VxIV,VyIV,VzIV,PxIV,PyIV,PzIV"),
LOG_FORMAT(EST5, "ffffffff", "MAGxI,MAGyI,MAGzI,MAGxIV,MAGyIV,MAGzIV,HeadI,HeadIV"),
LOG_FORMAT(EST6, "ffff", "FxI,FyI,FxIV,FyIV"),
LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"),
LOG_FORMAT(MOCP, "fffffff", "QuatW,QuatX,QuatY,QuatZ,X,Y,Z"),
LOG_FORMAT(VISN, "ffffffffff", "X,Y,Z,VX,VY,VZ,QuatW,QuatX,QuatY,QuatZ"),