From 01ee6081177fdf5f303d546046b62a2997bde34f Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Tue, 23 Feb 2016 13:07:40 -0800 Subject: [PATCH] sdlog: setup flow innovation logging --- msg/ekf2_innovations.msg | 2 ++ src/modules/ekf2/ekf2_main.cpp | 3 ++- src/modules/sdlog2/sdlog2.c | 9 +++++++++ src/modules/sdlog2/sdlog2_messages.h | 9 ++++++++- 4 files changed, 21 insertions(+), 2 deletions(-) diff --git a/msg/ekf2_innovations.msg b/msg/ekf2_innovations.msg index cb2ab03146..5d635eb31d 100644 --- a/msg/ekf2_innovations.msg +++ b/msg/ekf2_innovations.msg @@ -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 \ No newline at end of file diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index dfacd2366c..67ca59be3c 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -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); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index c6e5adf10f..eb5005590f 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -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 --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 526d30227a..f4a862e515 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -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"),