fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-29 15:54:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

sdlog2: DIST (distance to surface) message added

Browse Source
This commit is contained in:
Anton Babushkin 2013-10-13 19:45:04 +02:00
parent 5d556f1850
commit 419cb4bc80
2 changed files with 26 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/modules/sdlog2/sdlog2.c
View File

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

15
src/modules/sdlog2/sdlog2_messages.h
View File

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