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

Added VICON logging, finally.

This commit is contained in:
Helen Oleynikova 2014-04-09 17:11:27 +02:00
parent 8e0e0c164a
commit 863cd2e838
2 changed files with 31 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -226,11 +226,11 @@ sdlog2_usage(const char *reason)
}
errx(1, "usage: sdlog2 {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a -t\n"
"\t-r\tLog rate in Hz, 0 means unlimited rate\n"
"\t-b\tLog buffer size in KiB, default is 8\n"
"\t-e\tEnable logging by default (if not, can be started by command)\n"
"\t-a\tLog only when armed (can be still overriden by command)\n"
"\t-t\tUse date/time for naming log directories and files\n");
"\t-r\tLog rate in Hz, 0 means unlimited rate\n"
"\t-b\tLog buffer size in KiB, default is 8\n"
"\t-e\tEnable logging by default (if not, can be started by command)\n"
"\t-a\tLog only when armed (can be still overriden by command)\n"
"\t-t\tUse date/time for naming log directories and files\n");
}
/**
@ -257,11 +257,11 @@ int sdlog2_main(int argc, char *argv[])
main_thread_should_exit = false;
deamon_task = task_spawn_cmd("sdlog2",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT - 30,
3000,
sdlog2_thread_main,
(const char **)argv);
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT - 30,
3000,
sdlog2_thread_main,
(const char **)argv);
exit(0);
}
@ -833,6 +833,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_TELE_s log_TELE;
struct log_ESTM_s log_ESTM;
struct log_PWR_s log_PWR;
struct log_VICON_s log_VICON;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
@ -1163,7 +1164,14 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- VICON POSITION --- */
if (copy_if_updated(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos)) {
// TODO not implemented yet
log_msg.msg_type = LOG_VICON_MSG;
log_msg.body.log_VICON.x = buf.vicon_pos.x;
log_msg.body.log_VICON.y = buf.vicon_pos.y;
log_msg.body.log_VICON.z = buf.vicon_pos.z;
log_msg.body.log_VICON.pitch = buf.vicon_pos.pitch;
log_msg.body.log_VICON.roll = buf.vicon_pos.roll;
log_msg.body.log_VICON.yaw = buf.vicon_pos.yaw;
LOGBUFFER_WRITE_AND_COUNT(VICON);
}
/* --- FLOW --- */

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

@ -301,6 +301,17 @@ struct log_PWR_s {
uint8_t high_power_rail_overcurrent;
};
/* --- VICON - VICON POSITION --- */
#define LOG_VICON_MSG 25
struct log_VICON_s {
float x;
float y;
float z;
float roll;
float pitch;
float yaw;
};
/********** SYSTEM MESSAGES, ID > 0x80 **********/
/* --- TIME - TIME STAMP --- */
@ -351,6 +362,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"),
LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"),
LOG_FORMAT(PWR, "fffBBBBB", "Periph_5V,Servo_5V,RSSI,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"),
LOG_FORMAT(VICON, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"),
/* system-level messages, ID >= 0x80 */
/* FMT: don't write format of format message, it's useless */