From d563960267ab1145d5100f9dcfad6035205e4021 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Jun 2013 17:24:04 +0200 Subject: Added Flow messages to sdlog2 --- src/modules/sdlog2/sdlog2.c | 10 ++++++++-- src/modules/sdlog2/sdlog2_messages.h | 10 ++++++++++ 2 files changed, 18 insertions(+), 2 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index edcba4e7d..33b3c657b 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -616,7 +616,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ /* number of messages */ - const ssize_t fdsc = 19; + const ssize_t fdsc = 20; /* Sanity check variable and index */ ssize_t fdsc_count = 0; @@ -686,6 +686,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_SENS_s log_SENS; struct log_LPOS_s log_LPOS; struct log_LPSP_s log_LPSP; + struct log_FLOW_s log_FLOW; struct log_GPS_s log_GPS; struct log_ATTC_s log_ATTC; struct log_STAT_s log_STAT; @@ -1129,7 +1130,12 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- FLOW --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow); - // TODO not implemented yet + log_msg.msg_type = LOG_FLOW_MSG; + log_msg.body.log_FLOW.flow_comp_x_m = buf.flow.flow_comp_x_m; + log_msg.body.log_FLOW.flow_comp_y_m = buf.flow.flow_comp_y_m; + log_msg.body.log_FLOW.ground_distance_m = buf.flow.ground_distance_m; + log_msg.body.log_FLOW.quality = buf.flow.quality; + LOGBUFFER_WRITE_AND_COUNT(FLOW); } /* --- RC CHANNELS --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 04eae40b0..755d21d55 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -206,6 +206,15 @@ struct log_ARSP_s { float pitch_rate_sp; float yaw_rate_sp; }; + +/* --- FLOW - FLOW DATA --- */ +#define LOG_FLOW_MSG 16 +struct log_FLOW_s { + float flow_comp_x_m; + float flow_comp_y_m; + float ground_distance_m; + uint8_t quality; +}; #pragma pack(pop) /* construct list of all message formats */ @@ -226,6 +235,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), + LOG_FORMAT(FLOW, "fffB", "FlowX,FlowY,SonAlt,FQual") }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); -- cgit v1.2.3