From 1c2f95697cc0789b576fc316e8edaae322e91e1c Mon Sep 17 00:00:00 2001
From: tzai <tal@talzaitsev.com>
Date: Wed, 1 Jan 2020 23:43:03 -0500
Subject: [PATCH] logger: Add support for logging only on bat power

Will not start logging from boot if battery is not connected

Signed-off-by: Tal Zaitsev <tal@corvus-robotics.com>
---
 src/modules/logger/logger.cpp | 27 ++++++++++++++++++++++++++-
 src/modules/logger/logger.h   |  1 +
 src/modules/logger/params.c   | 15 +++++++++++++++
 3 files changed, 42 insertions(+), 1 deletion(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 4ff48245ca..5166f1ad6e 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -49,6 +49,7 @@
 #include <uORB/uORBTopics.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_command_ack.h>
+#include <uORB/topics/battery_status.h>
 
 #include <drivers/drv_hrt.h>
 #include <mathlib/math/Limits.hpp>
@@ -374,6 +375,7 @@ Logger::Logger(LogWriter::Backend backend, size_t buffer_size, uint32_t log_inte
 	_log_dirs_max = param_find("SDLOG_DIRS_MAX");
 	_sdlog_profile_handle = param_find("SDLOG_PROFILE");
 	_mission_log = param_find("SDLOG_MISSION");
+	_boot_bat_only = param_find("SDLOG_BOOT_BAT");
 
 	if (poll_topic_name) {
 		const orb_metadata *const *topics = orb_get_topics();
@@ -518,6 +520,29 @@ void Logger::run()
 {
 	PX4_INFO("logger started (mode=%s)", configured_backend_mode());
 
+	bool boot_logging_bat_only = false;
+	bool disable_boot_logging = false;
+
+	if (_boot_bat_only != PARAM_INVALID) {
+		param_get(_boot_bat_only, &boot_logging_bat_only);
+	}
+
+	if (boot_logging_bat_only) {
+		uORB::Subscription battery_status_sub{ORB_ID(battery_status)};
+
+		if (battery_status_sub.updated()) {
+			battery_status_s battery_status;
+			battery_status_sub.copy(&battery_status);
+
+			if (!battery_status.connected) {
+				disable_boot_logging = true;
+			}
+
+		} else {
+			PX4_WARN("battery_status not published. Logging anyway");
+		}
+	}
+
 	if (_writer.backend() & LogWriter::BackendFile) {
 		int mkdir_ret = mkdir(LOG_ROOT[(int)LogType::Full], S_IRWXU | S_IRWXG | S_IRWXO);
 
@@ -602,7 +627,7 @@ void Logger::run()
 
 	px4_register_shutdown_hook(&Logger::request_stop_static);
 
-	if (_log_mode == LogMode::boot_until_disarm || _log_mode == LogMode::boot_until_shutdown) {
+	if ((_log_mode == LogMode::boot_until_disarm || _log_mode == LogMode::boot_until_shutdown) && !disable_boot_logging) {
 		start_log_file(LogType::Full);
 	}
 
diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h
index 39c55db892..e67362f305 100644
--- a/src/modules/logger/logger.h
+++ b/src/modules/logger/logger.h
@@ -338,6 +338,7 @@ private:
 	param_t						_log_utc_offset{PARAM_INVALID};
 	param_t						_log_dirs_max{PARAM_INVALID};
 	param_t						_mission_log{PARAM_INVALID};
+	param_t						_boot_bat_only{PARAM_INVALID};
 };
 
 } //namespace logger
diff --git a/src/modules/logger/params.c b/src/modules/logger/params.c
index 3da2362510..1ac7f248fd 100644
--- a/src/modules/logger/params.c
+++ b/src/modules/logger/params.c
@@ -66,6 +66,21 @@ PARAM_DEFINE_INT32(SDLOG_UTC_OFFSET, 0);
  */
 PARAM_DEFINE_INT32(SDLOG_MODE, 0);
 
+/**
+ * Battery-only Logging
+ *
+ * When enabled, logging will not start from boot if battery power is not detected
+ * (e.g. powered via USB on a test bench). This prevents extraneous flight logs from
+ * being created during bench testing.
+ *
+ * Note that this only applies to log-from-boot modes. This has no effect on arm-based
+ * modes.
+ *
+ * @boolean
+ * @group SD Logging
+ */
+PARAM_DEFINE_INT32(SDLOG_BOOT_BAT, 0);
+
 /**
  * Mission Log
  *