diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-06-07 21:33:49 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-06-07 21:33:49 +0200 |
commit | 048967fb6f30a413d560e95265461ae8c7dc74cd (patch) | |
tree | 2393db5f4f7c6a92d5c3d01ae49a6fc8ca4461dd /src/modules | |
parent | 05e77d98489cb094be3e777c0eaf97d3e13b5f69 (diff) | |
parent | 5a9e52a287d50d3818d145a61a52197761cf14b7 (diff) | |
download | px4-firmware-048967fb6f30a413d560e95265461ae8c7dc74cd.tar.gz px4-firmware-048967fb6f30a413d560e95265461ae8c7dc74cd.tar.bz2 px4-firmware-048967fb6f30a413d560e95265461ae8c7dc74cd.zip |
merged
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/sdlog2/logbuffer.c | 3 | ||||
-rw-r--r-- | src/modules/sdlog2/logbuffer.h | 4 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2.c | 93 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2_messages.h | 25 |
4 files changed, 100 insertions, 25 deletions
diff --git a/src/modules/sdlog2/logbuffer.c b/src/modules/sdlog2/logbuffer.c index 4205bcf20..8aaafaf31 100644 --- a/src/modules/sdlog2/logbuffer.c +++ b/src/modules/sdlog2/logbuffer.c @@ -45,12 +45,13 @@ #include "logbuffer.h" -void logbuffer_init(struct logbuffer_s *lb, int size) +int logbuffer_init(struct logbuffer_s *lb, int size) { lb->size = size; lb->write_ptr = 0; lb->read_ptr = 0; lb->data = malloc(lb->size); + return (lb->data == 0) ? ERROR : OK; } int logbuffer_count(struct logbuffer_s *lb) diff --git a/src/modules/sdlog2/logbuffer.h b/src/modules/sdlog2/logbuffer.h index e9635e5e7..31521f722 100644 --- a/src/modules/sdlog2/logbuffer.h +++ b/src/modules/sdlog2/logbuffer.h @@ -46,14 +46,14 @@ #include <stdbool.h> struct logbuffer_s { - // all pointers are in bytes + // pointers and size are in bytes int write_ptr; int read_ptr; int size; char *data; }; -void logbuffer_init(struct logbuffer_s *lb, int size); +int logbuffer_init(struct logbuffer_s *lb, int size); int logbuffer_count(struct logbuffer_s *lb); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 3a06cef65..d9ba02830 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -94,9 +94,9 @@ } #define LOG_ORB_SUBSCRIBE(_var, _topic) subs.##_var##_sub = orb_subscribe(ORB_ID(##_topic##)); \ - fds[fdsc_count].fd = subs.##_var##_sub; \ - fds[fdsc_count].events = POLLIN; \ - fdsc_count++; + fds[fdsc_count].fd = subs.##_var##_sub; \ + fds[fdsc_count].events = POLLIN; \ + fdsc_count++; //#define SDLOG2_DEBUG @@ -107,7 +107,7 @@ static int deamon_task; /**< Handle of deamon task / thread */ static bool logwriter_should_exit = false; /**< Logwriter thread exit flag */ static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log folders */ static const int MAX_NO_LOGFILE = 999; /**< Maximum number of log files */ -static const int LOG_BUFFER_SIZE = 8192; +static const int LOG_BUFFER_SIZE_DEFAULT = 8192; static const int MAX_WRITE_CHUNK = 512; static const int MIN_BYTES_TO_WRITE = 512; @@ -207,8 +207,9 @@ sdlog2_usage(const char *reason) if (reason) fprintf(stderr, "%s\n", reason); - errx(1, "usage: sdlog2 {start|stop|status} [-r <log rate>] -e -a\n" + errx(1, "usage: sdlog2 {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a\n" "\t-r\tLog rate in Hz, 0 means unlimited rate\n" + "\t-r\tLog buffer size in KBytes, 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"); } @@ -484,7 +485,7 @@ void sdlog2_stop_log() warnx("stop logging."); mavlink_log_info(mavlink_fd, "[sdlog2] stop logging"); - logging_enabled = true; + logging_enabled = false; logwriter_should_exit = true; /* wake up write thread one last time */ @@ -529,18 +530,18 @@ int sdlog2_thread_main(int argc, char *argv[]) warnx("failed to open MAVLink log stream, start mavlink app first."); } - /* log every n'th value (skip three per default) */ - int skip_value = 3; + /* log buffer size */ + int log_buffer_size = LOG_BUFFER_SIZE_DEFAULT; /* work around some stupidity in task_create's argv handling */ argc -= 2; argv += 2; int ch; - while ((ch = getopt(argc, argv, "r:ea")) != EOF) { + while ((ch = getopt(argc, argv, "r:b:ea")) != EOF) { switch (ch) { case 'r': { - unsigned r = strtoul(optarg, NULL, 10); + unsigned long r = strtoul(optarg, NULL, 10); if (r == 0) { sleep_delay = 0; @@ -551,6 +552,17 @@ int sdlog2_thread_main(int argc, char *argv[]) } break; + case 'b': { + unsigned long s = strtoul(optarg, NULL, 10); + + if (s < 1) { + s = 1; + } + + log_buffer_size = 1024 * s; + } + break; + case 'e': log_on_start = true; break; @@ -572,7 +584,7 @@ int sdlog2_thread_main(int argc, char *argv[]) default: sdlog2_usage("unrecognized flag"); - errx(1, "exiting"); + errx(1, "exiting."); } } @@ -580,12 +592,20 @@ int sdlog2_thread_main(int argc, char *argv[]) errx(1, "logging mount point %s not present, exiting.", mountpoint); } - if (create_logfolder()) + if (create_logfolder()) { errx(1, "unable to create logging folder, exiting."); + } /* only print logging path, important to find log file later */ warnx("logging to directory: %s", folder_path); + /* initialize log buffer with specified size */ + warnx("log buffer size: %i bytes.", log_buffer_size); + + if (OK != logbuffer_init(&lb, log_buffer_size)) { + errx(1, "can't allocate log buffer, exiting."); + } + /* file descriptors to wait for */ struct pollfd fds_control[2]; @@ -597,10 +617,12 @@ int sdlog2_thread_main(int argc, char *argv[]) /* file descriptors to wait for */ struct pollfd fds[fdsc]; + struct vehicle_status_s buf_status; + memset(&buf_status, 0, sizeof(buf_status)); + /* warning! using union here to save memory, elements should be used separately! */ union { struct vehicle_command_s cmd; - struct vehicle_status_s status; struct sensor_combined_s sensor; struct vehicle_attitude_s att; struct vehicle_attitude_setpoint_s att_sp; @@ -652,6 +674,8 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_LPOS_s log_LPOS; struct log_LPSP_s log_LPSP; struct log_GPS_s log_GPS; + struct log_ATTC_s log_ATTC; + struct log_STAT_s log_STAT; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -766,9 +790,6 @@ int sdlog2_thread_main(int argc, char *argv[]) thread_running = true; - /* initialize log buffer with specified size */ - logbuffer_init(&lb, LOG_BUFFER_SIZE); - /* initialize thread synchronization */ pthread_mutex_init(&logbuffer_mutex, NULL); pthread_cond_init(&logbuffer_cond, NULL); @@ -802,25 +823,32 @@ int sdlog2_thread_main(int argc, char *argv[]) * logging_enabled can be changed while checking vehicle_command and vehicle_status */ bool check_data = logging_enabled; int ifds = 0; + int handled_topics = 0; - /* --- VEHICLE COMMAND --- */ + /* --- VEHICLE COMMAND - LOG MANAGEMENT --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd); handle_command(&buf.cmd); + handled_topics++; } - /* --- VEHICLE STATUS --- */ + /* --- VEHICLE STATUS - LOG MANAGEMENT --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf.status); + orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status); + if (log_when_armed) { - handle_status(&buf.status); + handle_status(&buf_status); } + + handled_topics++; } - if (!logging_enabled || !check_data) { + if (!logging_enabled || !check_data || handled_topics >= poll_ret) { continue; } + ifds = 1; // Begin from fds[1] again + pthread_mutex_lock(&logbuffer_mutex); /* write time stamp message */ @@ -828,6 +856,22 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_TIME.t = hrt_absolute_time(); LOGBUFFER_WRITE_AND_COUNT(TIME); + /* --- VEHICLE STATUS --- */ + if (fds[ifds++].revents & POLLIN) { + // Don't orb_copy, it's already done few lines above + log_msg.msg_type = LOG_STAT_MSG; + log_msg.body.log_STAT.state = (unsigned char) buf_status.state_machine; + log_msg.body.log_STAT.flight_mode = (unsigned char) buf_status.flight_mode; + log_msg.body.log_STAT.manual_control_mode = (unsigned char) buf_status.manual_control_mode; + log_msg.body.log_STAT.manual_sas_mode = (unsigned char) buf_status.manual_sas_mode; + log_msg.body.log_STAT.armed = (unsigned char) buf_status.flag_system_armed; + log_msg.body.log_STAT.battery_voltage = buf_status.voltage_battery; + log_msg.body.log_STAT.battery_current = buf_status.current_battery; + log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; + log_msg.body.log_STAT.battery_warning = (unsigned char) buf_status.battery_warning; + LOGBUFFER_WRITE_AND_COUNT(STAT); + } + /* --- GPS POSITION --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); @@ -930,7 +974,12 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- ACTUATOR CONTROL --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.act_controls_sub, &buf.act_controls); - // TODO not implemented yet + log_msg.msg_type = LOG_ATTC_MSG; + log_msg.body.log_ATTC.roll = buf.act_controls.control[0]; + log_msg.body.log_ATTC.pitch = buf.act_controls.control[1]; + log_msg.body.log_ATTC.yaw = buf.act_controls.control[2]; + log_msg.body.log_ATTC.thrust = buf.act_controls.control[3]; + LOGBUFFER_WRITE_AND_COUNT(ATTC); } /* --- ACTUATOR CONTROL EFFECTIVE --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 316459b1e..2b6b86521 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -132,6 +132,29 @@ struct log_GPS_s { float vel_d; float cog; }; + +/* --- ATTC - ATTITUDE CONTROLS --- */ +#define LOG_ATTC_MSG 9 +struct log_ATTC_s { + float roll; + float pitch; + float yaw; + float thrust; +}; + +/* --- STAT - VEHICLE STATE --- */ +#define LOG_STAT_MSG 10 +struct log_STAT_s { + unsigned char state; + unsigned char flight_mode; + unsigned char manual_control_mode; + unsigned char manual_sas_mode; + unsigned char armed; + float battery_voltage; + float battery_current; + float battery_remaining; + unsigned char battery_warning; +}; #pragma pack(pop) /* construct list of all message formats */ @@ -145,6 +168,8 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(LPOS, "fffffffLLf", "X,Y,Z,VX,VY,VZ,Heading,HomeLat,HomeLon,HomeAlt"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), + LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), + LOG_FORMAT(STAT, "BBBBBfffB", "State,FlightMode,CtlMode,SASMode,Armed,BatV,BatC,BatRem,BatWarn"), }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); |