merged

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);