From 419cb4bc80d58928d632d0397db75de58534d685 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 13 Oct 2013 19:45:04 +0200 Subject: sdlog2: DIST (distance to surface) message added --- src/modules/sdlog2/sdlog2.c | 17 +++++++++++++++-- src/modules/sdlog2/sdlog2_messages.h | 15 +++++++++++---- 2 files changed, 26 insertions(+), 6 deletions(-) (limited to 'src/modules/sdlog2') 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); -- cgit v1.2.3