sdlog: setup flow innovation logging

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

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);
 

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

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"),