From 650de90a904368eb93689bbb8a3be63888bf244e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Jun 2013 14:40:55 +0400 Subject: sdlog2: ARSP, GPOS messages added --- src/modules/sdlog2/sdlog2.c | 18 ++++++++++++------ src/modules/sdlog2/sdlog2_messages.h | 27 +++++++++++++++++++++------ 2 files changed, 33 insertions(+), 12 deletions(-) (limited to 'src') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index e93b934c8..65cfdc188 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -608,12 +608,9 @@ int sdlog2_thread_main(int argc, char *argv[]) errx(1, "can't allocate log buffer, exiting."); } - /* file descriptors to wait for */ - struct pollfd fds_control[2]; - /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ /* number of messages */ - const ssize_t fdsc = 16; + const ssize_t fdsc = 17; /* Sanity check variable and index */ ssize_t fdsc_count = 0; /* file descriptors to wait for */ @@ -681,8 +678,9 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_STAT_s log_STAT; struct log_RC_s log_RC; struct log_OUT0_s log_OUT0; - struct log_ARSP_s log_ARSP; struct log_AIRS_s log_AIRS; + struct log_ARSP_s log_ARSP; + struct log_GPOS_s log_GPOS; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1053,7 +1051,15 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GLOBAL POSITION --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos); - // TODO not implemented yet + log_msg.msg_type = LOG_GPOS_MSG; + log_msg.body.log_GPOS.lat = buf.global_pos.lat; + log_msg.body.log_GPOS.lon = buf.global_pos.lon; + log_msg.body.log_GPOS.alt = buf.global_pos.alt; + log_msg.body.log_GPOS.vel_n = buf.global_pos.vx; + log_msg.body.log_GPOS.vel_e = buf.global_pos.vy; + log_msg.body.log_GPOS.vel_d = buf.global_pos.vz; + log_msg.body.log_GPOS.hdg = buf.global_pos.hdg; + LOGBUFFER_WRITE_AND_COUNT(GPOS); } /* --- VICON POSITION --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 5eeebcd95..c47c388c4 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -171,20 +171,33 @@ struct log_OUT0_s { float output[8]; }; +/* --- AIRS - AIRSPEED --- */ +#define LOG_AIRS_MSG 13 +struct log_AIRS_s { + float indicated_airspeed; + float true_airspeed; +}; + /* --- ARSP - ATTITUDE RATE SET POINT --- */ -#define LOG_ARSP_MSG 13 +#define LOG_ARSP_MSG 14 struct log_ARSP_s { float roll_rate_sp; float pitch_rate_sp; float yaw_rate_sp; }; -/* --- AIRS - AIRSPEED --- */ -#define LOG_AIRS_MSG 13 -struct log_AIRS_s { - float indicated_airspeed; - float true_airspeed; +/* --- GPOS - GLOBAL POSITION --- */ +#define LOG_GPOS_MSG 15 +struct log_GPOS_s { + int32_t lat; + int32_t lon; + float alt; + float vel_n; + float vel_e; + float vel_d; + float hdg; }; + #pragma pack(pop) /* construct list of all message formats */ @@ -203,6 +216,8 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"), 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(GPOS, "LLfffff", "Lat,Lon,Alt,VelN,VelE,VelD"), }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); -- cgit v1.2.3