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

Merge pull request #318 from PX4/global_pos_log 

Added global position to logging
This commit is contained in:
sjwilks 2013-06-30 11:53:44 -07:00
parent 8191130bbc aa04701c89
commit bff6eae3a2
2 changed files with 21 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -685,6 +685,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_AIRS_s log_AIRS; struct log_AIRS_s log_AIRS;
struct log_ARSP_s log_ARSP; struct log_ARSP_s log_ARSP;
struct log_FLOW_s log_FLOW; struct log_FLOW_s log_FLOW;
struct log_GPOS_s log_GPOS;
} body; } body;
} log_msg = { } log_msg = {
LOG_PACKET_HEADER_INIT(0) LOG_PACKET_HEADER_INIT(0)
@ -1056,7 +1057,14 @@ 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;
LOGBUFFER_WRITE_AND_COUNT(GPOS);
} }
/* --- VICON POSITION --- */ /* --- VICON POSITION --- */

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

@ -198,6 +198,17 @@ struct log_FLOW_s {
uint8_t quality; uint8_t quality;
uint8_t sensor_id; uint8_t sensor_id;
}; };
/* --- GPOS - GLOBAL POSITION ESTIMATE --- */
#define LOG_GPOS_MSG 16
struct log_GPOS_s {
int32_t lat;
int32_t lon;
float alt;
float vel_n;
float vel_e;
float vel_d;
};
#pragma pack(pop) #pragma pack(pop)
/* construct list of all message formats */ /* construct list of all message formats */
@ -218,6 +229,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
LOG_FORMAT(GPOS, "LLffff", "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);