Merge branch 'master' into autostart_cleanup

2 files changed, 35 insertions, 9 deletions

diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 658c6779c..ad9726863 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *   Author: Lorenz Meier <lm@inf.ethz.ch>
  *           Anton Babushkin <anton.babushkin@me.com>
  *
@@ -702,6 +702,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct airspeed_s airspeed;
 		struct esc_status_s esc;
 		struct vehicle_global_velocity_setpoint_s global_vel_sp;
+		struct battery_status_s battery;
 	} buf;
 
 	memset(&buf, 0, sizeof(buf));
@@ -726,6 +727,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int airspeed_sub;
 		int esc_sub;
 		int global_vel_sp_sub;
+		int battery_sub;
 	} subs;
 
 	/* log message buffer: header + body */
@@ -752,6 +754,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_GPSP_s log_GPSP;
 			struct log_ESC_s log_ESC;
 			struct log_GVSP_s log_GVSP;
+			struct log_BATT_s log_BATT;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -760,9 +763,9 @@ int sdlog2_thread_main(int argc, char *argv[])
 	memset(&log_msg.body, 0, sizeof(log_msg.body));
 
 	/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
-	/* number of subscriptions */
-	const ssize_t fdsc = 19;
-	/* sanity check variable and index */
+	/* number of messages */
+	const ssize_t fdsc = 25;
+	/* Sanity check variable and index */
 	ssize_t fdsc_count = 0;
 	/* file descriptors to wait for */
 	struct pollfd fds[fdsc];
@@ -881,6 +884,12 @@ int sdlog2_thread_main(int argc, char *argv[])
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
+	/* --- BATTERY --- */
+	subs.battery_sub = orb_subscribe(ORB_ID(battery_status));
+	fds[fdsc_count].fd = subs.battery_sub;
+	fds[fdsc_count].events = POLLIN;
+	fdsc_count++;
+
 	/* WARNING: If you get the error message below,
 	 * then the number of registered messages (fdsc)
 	 * differs from the number of messages in the above list.
@@ -971,8 +980,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 				log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state;
 				log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state;
 				log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state;
-				log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage;
-				log_msg.body.log_STAT.battery_current = buf_status.battery_current;
 				log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
 				log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning;
 				log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed;
@@ -1238,6 +1245,17 @@ int sdlog2_thread_main(int argc, char *argv[])
 				LOGBUFFER_WRITE_AND_COUNT(GVSP);
 			}
 
+			/* --- BATTERY --- */
+			if (fds[ifds++].revents & POLLIN) {
+				orb_copy(ORB_ID(battery_status), subs.battery_sub, &buf.battery);
+				log_msg.msg_type = LOG_BATT_MSG;
+				log_msg.body.log_BATT.voltage = buf.battery.voltage_v;
+				log_msg.body.log_BATT.voltage_filtered = buf.battery.voltage_filtered_v;
+				log_msg.body.log_BATT.current = buf.battery.current_a;
+				log_msg.body.log_BATT.discharged = buf.battery.discharged_mah;
+				LOGBUFFER_WRITE_AND_COUNT(BATT);
+			}
+
 			/* signal the other thread new data, but not yet unlock */
 			if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
 				/* only request write if several packets can be written at once */
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index ab4dc9b00..a784a1f30 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -148,8 +148,6 @@ struct log_STAT_s {
 	uint8_t main_state;
 	uint8_t navigation_state;
 	uint8_t arming_state;
-	float battery_voltage;
-	float battery_current;
 	float battery_remaining;
 	uint8_t battery_warning;
 	uint8_t landed;
@@ -248,6 +246,15 @@ struct log_GVSP_s {
 	float vz;
 };
 
+/* --- BATT - BATTERY --- */
+#define LOG_BATT_MSG 20
+struct log_BATT_s {
+	float voltage;
+	float voltage_filtered;
+	float current;
+	float discharged;
+};
+
 /* --- TIME - TIME STAMP --- */
 #define LOG_TIME_MSG 129
 struct log_TIME_s {
@@ -281,7 +288,7 @@ static const struct log_format_s log_formats[] = {
 	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, "BBBfffBB", "MainState,NavState,ArmState,BatV,BatC,BatRem,BatWarn,Landed"),
+	LOG_FORMAT(STAT, "BBBfBB", "MainState,NavState,ArmState,BatRem,BatWarn,Landed"),
 	LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"),
 	LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
 	LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
@@ -291,6 +298,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"),
 	LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
 	LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
+	LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
 	/* system-level messages, ID >= 0x80 */
 	// FMT: don't write format of format message, it's useless
 	LOG_FORMAT(TIME, "Q", "StartTime"),