sdlog2: DIST (distance to surface) message added

2 files changed, 26 insertions, 6 deletions

diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 7541910d1..76ff27e56 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -668,6 +668,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_GPSP_s log_GPSP;
 			struct log_ESC_s log_ESC;
 			struct log_GVSP_s log_GVSP;
+			struct log_DIST_s log_DIST;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -831,6 +832,9 @@ int sdlog2_thread_main(int argc, char *argv[])
 	uint16_t baro_counter = 0;
 	uint16_t differential_pressure_counter = 0;
 
+	/* track changes in distance status */
+	bool dist_bottom_present = false;
+
 	/* enable logging on start if needed */
 	if (log_on_start)
 		sdlog2_start_log();
@@ -1046,12 +1050,21 @@ int sdlog2_thread_main(int argc, char *argv[])
 				log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat;
 				log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon;
 				log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt;
-				log_msg.body.log_LPOS.dist_bottom = buf.local_pos.dist_bottom;
 				log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0);
 				log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0);
-				log_msg.body.log_LPOS.dist_flags = (buf.local_pos.dist_bottom_valid ? 1 : 0);
 				log_msg.body.log_LPOS.landed = buf.local_pos.landed;
 				LOGBUFFER_WRITE_AND_COUNT(LPOS);
+
+				if (buf.local_pos.dist_bottom_valid) {
+					dist_bottom_present = true;
+				}
+				if (dist_bottom_present) {
+					log_msg.msg_type = LOG_DIST_MSG;
+					log_msg.body.log_DIST.bottom = buf.local_pos.dist_bottom;
+					log_msg.body.log_DIST.bottom_rate = buf.local_pos.dist_bottom_rate;
+					log_msg.body.log_DIST.flags = (buf.local_pos.dist_bottom_valid ? 1 : 0);
+					LOGBUFFER_WRITE_AND_COUNT(DIST);
+				}
 			}
 
 			/* --- LOCAL POSITION SETPOINT --- */
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 32dd4f714..7e11940b7 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -109,10 +109,8 @@ struct log_LPOS_s {
 	int32_t ref_lat;
 	int32_t ref_lon;
 	float ref_alt;
-	float dist_bottom;
 	uint8_t xy_flags;
 	uint8_t z_flags;
-	uint8_t dist_flags;
 	uint8_t landed;
 };
 
@@ -261,6 +259,14 @@ struct log_FWRV_s {
 	char    fw_revision[64];
 };
 
+/* --- DIST - DISTANCE TO SURFACE --- */
+#define LOG_DIST_MSG 21
+struct log_DIST_s {
+	float bottom;
+	float bottom_rate;
+	uint8_t flags;
+};
+
 #pragma pack(pop)
 
 
@@ -283,7 +289,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
 	LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
 	LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"),
-	LOG_FORMAT(LPOS, "ffffffLLffBBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,DstB,XYFl,ZFl,DstFl,Land"),
+	LOG_FORMAT(LPOS, "ffffffLLfBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,XYFlags,ZFlags,Landed"),
 	LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
 	LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
 	LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
@@ -297,7 +303,8 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"),
 	LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
 	LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
-	LOG_FORMAT(FWRV,"Z",FW_VERSION_STR),
+	LOG_FORMAT(FWRV, "Z", FW_VERSION_STR),
+	LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"),
 };
 
 static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);