ekf2: Added airspeed to rpl logging

msg/ekf2_replay.msg

@@ -6,6 +6,7 @@ uint64 magnetometer_timestamp 		# timestamp of magnetometer measurement in us
 uint64 baro_timestamp			# timestamp of barometer measurement in us
 uint64 rng_timestamp			# timestamp of range finder measurement in us
 uint64 flow_timestamp			# timestamp of optical flow measurement in us
+uint64 asp_timestamp			# timestamp of airspeed measurement in us
 
 float32[3] gyro_integral_rad		# integrated gyro vector in rad
 float32[3] accelerometer_integral_m_s  	# integrated accelerometer vector in m/s
@@ -37,3 +38,11 @@ float32[2] flow_pixel_integral		# integrated optical flow rate around x and y ax
 float32[2] flow_gyro_integral		# integrated gyro rate around x and y axes (rad)
 uint32 flow_time_integral		# integration timespan (usec)
 uint8 flow_quality			# Quality of accumulated optical flow data (0 - 255)
+
+# airspeed
+float32 indicated_airspeed_m_s		# indicated airspeed in meters per second, -1 if unknown
+float32 true_airspeed_m_s		# true filtered airspeed in meters per second, -1 if unknown
+float32 true_airspeed_unfiltered_m_s	# true airspeed in meters per second, -1 if unknown
+float32 air_temperature_celsius		# air temperature in degrees celsius, -1000 if unknown
+float32 confidence			# confidence value from 0 to 1 for this sensor
+

src/modules/ekf2/ekf2_main.cpp

@@ -848,6 +848,19 @@ void Ekf2::task_main()
 				replay.rng_timestamp = 0;
 			}
 
+			if (airspeed_updated) {
+				replay.asp_timestamp = airspeed.timestamp;
+				replay.indicated_airspeed_m_s = airspeed.indicated_airspeed_m_s;
+				replay.true_airspeed_m_s = airspeed.true_airspeed_m_s;
+				replay.true_airspeed_unfiltered_m_s = airspeed.true_airspeed_unfiltered_m_s;
+				replay.air_temperature_celsius = airspeed.air_temperature_celsius;
+				replay.confidence = airspeed.confidence;
+
+			} else {
+				replay.asp_timestamp = 0;
+			}
+
+
 			if (_replay_pub == nullptr) {
 				_replay_pub = orb_advertise(ORB_ID(ekf2_replay), &replay);
 

src/modules/sdlog2/sdlog2.c

@@ -1230,6 +1230,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_EST6_s log_INO3;
 			struct log_RPL3_s log_RPL3;
 			struct log_RPL4_s log_RPL4;
+			struct log_RPL6_s log_RPL6;
 			struct log_LAND_s log_LAND;
 		} body;
 	} log_msg = {
@@ -1532,6 +1533,19 @@ int sdlog2_thread_main(int argc, char *argv[])
 				LOGBUFFER_WRITE_AND_COUNT(RPL4);
 			}
 
+
+			if (buf.replay.asp_timestamp > 0) {
+				log_msg.msg_type = LOG_RPL6_MSG;
+				log_msg.body.log_RPL6.timestamp = buf.replay.asp_timestamp;
+				log_msg.body.log_RPL6.indicated_airspeed_m_s = buf.replay.indicated_airspeed_m_s;
+				log_msg.body.log_RPL6.true_airspeed_m_s = buf.replay.true_airspeed_m_s;
+				log_msg.body.log_RPL6.true_airspeed_unfiltered_m_s = buf.replay.true_airspeed_unfiltered_m_s;
+				log_msg.body.log_RPL6.air_temperature_celsius = buf.replay.air_temperature_celsius;
+				log_msg.body.log_RPL6.confidence = buf.replay.confidence;
+				LOGBUFFER_WRITE_AND_COUNT(RPL6);
+			}
+
+
 		} else { /* !record_replay_log */
 
 			// we poll on sensor combined, so we know it has updated just now

src/modules/sdlog2/sdlog2_messages.h

@@ -576,6 +576,18 @@ struct log_RPL4_s {
 	float range_to_ground;
 };
 
+/* --- EKF2 REPLAY Part 4 --- */
+#define LOG_RPL6_MSG 59
+struct log_RPL6_s {
+	uint64_t timestamp;
+	float indicated_airspeed_m_s;
+	float true_airspeed_m_s;
+	float true_airspeed_unfiltered_m_s;
+	float air_temperature_celsius;
+	float confidence;
+};
+
+
 
 /* --- CAMERA TRIGGER --- */
 #define LOG_CAMT_MSG 55
@@ -673,6 +685,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(RPL2, "QQLLiMMfffffffM", "Tpos,Tvel,lat,lon,alt,fix,nsats,eph,epv,sacc,v,vN,vE,vD,v_val"),
 	LOG_FORMAT(RPL3, "QffffIB", "Tflow,fx,fy,gx,gy,delT,qual"),
 	LOG_FORMAT(RPL4, "Qf", "Trng,rng"),
+	LOG_FORMAT(RPL6, "Qfffff", "Tasp,inAsp,trAsp,ufAsp,tpAsp,confAsp"),
 	LOG_FORMAT(LAND, "B", "Landed"),
 	/* system-level messages, ID >= 0x80 */
 	/* FMT: don't write format of format message, it's useless */