From 9dbbe8cd8d1cb061d907a9dd62ba0dd47dcc3662 Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Sat, 30 Apr 2016 17:36:38 -0600
Subject: [PATCH] log changes to parameters

---
 src/modules/logger/logger.cpp | 76 +++++++++++++++++++++++++++++++++++
 src/modules/logger/logger.h   |  3 ++
 2 files changed, 79 insertions(+)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 7b664173e7..927448b3bf 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -452,6 +452,14 @@ void Logger::run()
 
 			bool data_written = false;
 
+			/* Check if parameters have changed */
+			// this needs to change to a timestamped record to record a history of parameter changes
+			if (_parameter_update_sub.check_updated()) {
+				warnx("parameter update");
+				_parameter_update_sub.update();
+				write_changed_parameters();
+			}
+
 			// Write data messages for normal subscriptions
 			int msg_id = 0;
 
@@ -757,11 +765,13 @@ void Logger::write_parameters()
 	param_t param = 0;
 
 	do {
+		// get next parameter which is invalid OR used
 		do {
 			param = param_for_index(param_idx);
 			++param_idx;
 		} while (param != PARAM_INVALID && !param_used(param));
 
+		// save parameters which are valid AND used
 		if (param != PARAM_INVALID) {
 			/* get parameter type and size */
 			const char *type_str;
@@ -808,5 +818,71 @@ void Logger::write_parameters()
 	_writer.notify();
 }
 
+void Logger::write_changed_parameters()
+{
+	_writer.lock();
+	uint8_t buffer[sizeof(message_parameter_header_s) + sizeof(param_value_u)];
+	message_parameter_header_s *msg = reinterpret_cast<message_parameter_header_s *>(buffer);
+
+	msg->msg_type = static_cast<uint8_t>(MessageType::PARAMETER);
+	int param_idx = 0;
+	param_t param = 0;
+
+	do {
+		// get next parameter which is invalid OR used
+		do {
+			param = param_for_index(param_idx);
+			++param_idx;
+		} while (param != PARAM_INVALID && !param_used(param));
+
+		// log parameters which are valid AND used AND unsaved
+		if ((param != PARAM_INVALID) && param_value_unsaved(param)) {
+			warnx("logging change to parameter %s", param_name(param));
+
+			/* get parameter type and size */
+			const char *type_str;
+			param_type_t type = param_type(param);
+			size_t value_size = 0;
+
+			switch (type) {
+			case PARAM_TYPE_INT32:
+				type_str = "int32_t";
+				value_size = sizeof(int32_t);
+				break;
+
+			case PARAM_TYPE_FLOAT:
+				type_str = "float";
+				value_size = sizeof(float);
+				break;
+
+			default:
+				continue;
+			}
+
+			/* format parameter key (type and name) */
+			msg->key_len = snprintf(msg->key, sizeof(msg->key), "%s %s", type_str, param_name(param));
+			size_t msg_size = sizeof(*msg) - sizeof(msg->key) + msg->key_len;
+
+			/* copy parameter value directly to buffer */
+			param_get(param, &buffer[msg_size]);
+			msg_size += value_size;
+
+			msg->msg_size = msg_size - 2;
+
+			/* write message */
+			while (!_writer.write(buffer, msg_size)) {
+				/* wait if buffer is full, don't skip PARAMETER messages */
+				_writer.unlock();
+				_writer.notify();
+				usleep(_log_interval);
+				_writer.lock();
+			}
+		}
+	} while ((param != PARAM_INVALID) && (param_idx < (int) param_count()));
+
+	_writer.unlock();
+	_writer.notify();
+}
+
 }
 }
diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h
index 67823309ff..f19de0ccb7 100644
--- a/src/modules/logger/logger.h
+++ b/src/modules/logger/logger.h
@@ -76,6 +76,8 @@ private:
 
 	void write_parameters();
 
+	void write_changed_parameters();
+
 	bool copy_if_updated_multi(orb_id_t topic, int multi_instance, int *handle, void *buffer, uint64_t *time_last_checked);
 
 	static constexpr size_t 	MAX_TOPICS_NUM = 128;
@@ -87,6 +89,7 @@ private:
 	uint8_t 					*_log_buffer;
 	char 						_log_dir[64];
 	uORB::Subscription<vehicle_status_s>	_vehicle_status_sub {ORB_ID(vehicle_status)};
+	uORB::Subscription<parameter_update_s>	_parameter_update_sub {ORB_ID(parameter_update)};
 	bool						_enabled = false;
 	bool 						_log_on_start;
 	Array<LoggerSubscription, MAX_TOPICS_NUM>	_subscriptions;