sdlog2: RC (RC controls) and OUT0 (actuator 0 output) messages added, print statistics to mavlink console

2 files changed, 38 insertions, 16 deletions

diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index b6ab8c2ed..a14bd6f80 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -77,6 +77,7 @@
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/airspeed.h>
+#include <uORB/topics/rc_channels.h>
 
 #include <systemlib/systemlib.h>
 
@@ -209,7 +210,7 @@ sdlog2_usage(const char *reason)
 
 	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-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");
 }
@@ -635,7 +636,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct vehicle_gps_position_s gps_pos;
 		struct vehicle_vicon_position_s vicon_pos;
 		struct optical_flow_s flow;
-		struct battery_status_s batt;
+		struct rc_channels_s rc;
 		struct differential_pressure_s diff_pres;
 		struct airspeed_s airspeed;
 	} buf;
@@ -656,9 +657,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int gps_pos_sub;
 		int vicon_pos_sub;
 		int flow_sub;
-		int batt_sub;
-		int diff_pres_sub;
-		int airspeed_sub;
+		int rc_sub;
 	} subs;
 
 	/* log message buffer: header + body */
@@ -676,6 +675,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_GPS_s log_GPS;
 			struct log_ATTC_s log_ATTC;
 			struct log_STAT_s log_STAT;
+			struct log_RC_s log_RC;
+			struct log_OUT0_s log_OUT0;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -720,7 +721,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	fdsc_count++;
 
 	/* --- ACTUATOR OUTPUTS --- */
-	subs.act_outputs_sub = orb_subscribe(ORB_ID(actuator_outputs_0));
+	subs.act_outputs_sub = orb_subscribe(ORB_ID_VEHICLE_CONTROLS);
 	fds[fdsc_count].fd = subs.act_outputs_sub;
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
@@ -767,9 +768,9 @@ int sdlog2_thread_main(int argc, char *argv[])
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
-	/* --- BATTERY STATUS --- */
-	subs.batt_sub = orb_subscribe(ORB_ID(battery_status));
-	fds[fdsc_count].fd = subs.batt_sub;
+	/* --- RC CHANNELS --- */
+	subs.rc_sub = orb_subscribe(ORB_ID(rc_channels));
+	fds[fdsc_count].fd = subs.rc_sub;
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
@@ -968,7 +969,9 @@ int sdlog2_thread_main(int argc, char *argv[])
 			/* --- ACTUATOR OUTPUTS --- */
 			if (fds[ifds++].revents & POLLIN) {
 				orb_copy(ORB_ID(actuator_outputs_0), subs.act_outputs_sub, &buf.act_outputs);
-				// TODO not implemented yet
+				log_msg.msg_type = LOG_OUT0_MSG;
+				memcpy(log_msg.body.log_OUT0.output, buf.act_outputs.output, sizeof(log_msg.body.log_OUT0.output));
+				LOGBUFFER_WRITE_AND_COUNT(OUT0);
 			}
 
 			/* --- ACTUATOR CONTROL --- */
@@ -1034,10 +1037,13 @@ int sdlog2_thread_main(int argc, char *argv[])
 				// TODO not implemented yet
 			}
 
-			/* --- BATTERY STATUS --- */
+			/* --- RC CHANNELS --- */
 			if (fds[ifds++].revents & POLLIN) {
-				orb_copy(ORB_ID(battery_status), subs.batt_sub, &buf.batt);
-				// TODO not implemented yet
+				orb_copy(ORB_ID(rc_channels), subs.rc_sub, &buf.rc);
+				log_msg.msg_type = LOG_RC_MSG;
+				/* Copy only the first 8 channels of 14 */
+				memcpy(log_msg.body.log_RC.channel, buf.rc.chan, sizeof(log_msg.body.log_RC.channel));
+				LOGBUFFER_WRITE_AND_COUNT(RC);
 			}
 
 			/* signal the other thread new data, but not yet unlock */
@@ -1073,10 +1079,12 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 void sdlog2_status()
 {
-	float mebibytes = log_bytes_written / 1024.0f / 1024.0f;
+	float kibibytes = log_bytes_written / 1024.0f;
+	float mebibytes = kibibytes / 1024.0f;
 	float seconds = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f;
 
-	warnx("wrote %lu msgs, %4.2f MiB (average %5.3f MiB/s), skipped %lu msgs.", log_msgs_written, (double)mebibytes, (double)(mebibytes / seconds), log_msgs_skipped);
+	warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs.", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped);
+	mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs.", log_msgs_written, log_msgs_skipped);
 }
 
 /**
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 2b6b86521..40763ee1e 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -133,7 +133,7 @@ struct log_GPS_s {
 	float cog;
 };
 
-/* --- ATTC - ATTITUDE CONTROLS --- */
+/* --- ATTC - ATTITUDE CONTROLS (ACTUATOR_0 CONTROLS)--- */
 #define LOG_ATTC_MSG 9
 struct log_ATTC_s {
 	float roll;
@@ -155,6 +155,18 @@ struct log_STAT_s {
 	float battery_remaining;
 	unsigned char battery_warning;
 };
+
+/* --- RC - RC INPUT CHANNELS --- */
+#define LOG_RC_MSG 11
+struct log_RC_s {
+	float channel[8];
+};
+
+/* --- OUT0 - ACTUATOR_0 OUTPUT --- */
+#define LOG_OUT0_MSG 12
+struct log_OUT0_s {
+	float output[8];
+};
 #pragma pack(pop)
 
 /* construct list of all message formats */
@@ -170,6 +182,8 @@ static const struct log_format_s log_formats[] = {
 	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"),
+	LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"),
+	LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
 };
 
 static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);