dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

sdlog2: ARSP, GPOS messages added

This commit is contained in:
Anton Babushkin 2013-06-17 14:40:55 +04:00
parent 95d324f061
commit 650de90a90
2 changed files with 35 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -608,12 +608,9 @@ int sdlog2_thread_main(int argc, char *argv[])
errx(1, "can't allocate log buffer, exiting."); 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 --- */ /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
/* number of messages */ /* number of messages */
const ssize_t fdsc = 16; const ssize_t fdsc = 17;
/* Sanity check variable and index */ /* Sanity check variable and index */
ssize_t fdsc_count = 0; ssize_t fdsc_count = 0;
/* file descriptors to wait for */ /* 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_STAT_s log_STAT;
struct log_RC_s log_RC; struct log_RC_s log_RC;
struct log_OUT0_s log_OUT0; struct log_OUT0_s log_OUT0;
struct log_ARSP_s log_ARSP;
struct log_AIRS_s log_AIRS; struct log_AIRS_s log_AIRS;
struct log_ARSP_s log_ARSP;
struct log_GPOS_s log_GPOS;
} body; } body;
} log_msg = { } log_msg = {
LOG_PACKET_HEADER_INIT(0) LOG_PACKET_HEADER_INIT(0)
@ -1053,7 +1051,15 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- GLOBAL POSITION --- */ /* --- GLOBAL POSITION --- */
if (fds[ifds++].revents & POLLIN) { if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos); 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 --- */ /* --- VICON POSITION --- */

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

@ -171,20 +171,33 @@ struct log_OUT0_s {
float output[8]; float output[8];
}; };
/* --- ARSP - ATTITUDE RATE SET POINT --- */
#define LOG_ARSP_MSG 13
struct log_ARSP_s {
float roll_rate_sp;
float pitch_rate_sp;
float yaw_rate_sp;
};
/* --- AIRS - AIRSPEED --- */ /* --- AIRS - AIRSPEED --- */
#define LOG_AIRS_MSG 13 #define LOG_AIRS_MSG 13
struct log_AIRS_s { struct log_AIRS_s {
float indicated_airspeed; float indicated_airspeed;
float true_airspeed; float true_airspeed;
}; };
/* --- ARSP - ATTITUDE RATE SET POINT --- */
#define LOG_ARSP_MSG 14
struct log_ARSP_s {
float roll_rate_sp;
float pitch_rate_sp;
float yaw_rate_sp;
};
/* --- 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) #pragma pack(pop)
/* construct list of all message formats */ /* 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(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"),
LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), 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); static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);