bizhang_-obav/src/modules/logger/logger.cpp

/****************************************************************************
 *
 *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

#include <px4_config.h>
#include "logger.h"
#include "messages.h"

#include <dirent.h>
#include <sys/stat.h>
#include <errno.h>
#include <string.h>
#include <stdlib.h>
#include <time.h>

#include <uORB/uORB.h>
#include <uORB/uORBTopics.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/log_message.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>

#include <drivers/drv_hrt.h>
#include <px4_includes.h>
#include <px4_getopt.h>
#include <px4_log.h>
#include <px4_posix.h>
#include <px4_sem.h>
#include <px4_shutdown.h>
#include <px4_tasks.h>
#include <systemlib/mavlink_log.h>
#include <replay/definitions.hpp>
#include <version/version.h>

#ifdef __PX4_DARWIN
#include <sys/param.h>
#include <sys/mount.h>
#else
#include <sys/statfs.h>
#endif

#define GPS_EPOCH_SECS ((time_t)1234567890ULL)

//#define DBGPRINT //write status output every few seconds

#if defined(DBGPRINT)
// needed for mallinfo
#if defined(__PX4_POSIX) && !defined(__PX4_DARWIN)
#include <malloc.h>
#endif /* __PX4_POSIX */

// struct mallinfo not available on OSX?
#if defined(__PX4_DARWIN)
#undef DBGPRINT
#endif /* defined(__PX4_DARWIN) */
#endif /* defined(DBGPRINT) */

using namespace px4::logger;

/* This is used to schedule work for the logger (periodic scan for updated topics) */
static void timer_callback(void *arg)
{
	px4_sem_t *semaphore = (px4_sem_t *)arg;
	px4_sem_post(semaphore);
}


int logger_main(int argc, char *argv[])
{
	// logger currently assumes little endian
	int num = 1;

	if (*(char *)&num != 1) {
		PX4_ERR("Logger only works on little endian!\n");
		return 1;
	}

	return Logger::main(argc, argv);
}

namespace px4
{
namespace logger
{

int Logger::custom_command(int argc, char *argv[])
{
	if (!is_running()) {
		print_usage("logger not running");
		return 1;
	}

	if (!strcmp(argv[0], "on")) {
		get_instance()->set_arm_override(true);
		return 0;
	}

	if (!strcmp(argv[0], "off")) {
		get_instance()->set_arm_override(false);
		return 0;
	}

	return print_usage("unknown command");
}

int Logger::print_usage(const char *reason)
{
	if (reason) {
		PX4_WARN("%s\n", reason);
	}

	PRINT_MODULE_DESCRIPTION(
		R"DESCR_STR(
System logger which logs a (configurable) set of uORB topics and system printf messages
(warnings and errors) to ULog files. These can be used for system and flight performance evaluation,
tuning, replay and crash analysis.

It supports 2 backends:
- Files: write ULog files to the file system (SD card)
- MAVLink: stream ULog data via MAVLink to a client (the client must support this)
Both backends can be enabled and used at the same time.

The implementation uses two threads:
- The main thread, running at a fixed rate (or polling on a topic if started with -p) and checking for
data updates
- The writer thread, writing data to the file
In between there is a write buffer with configurable size. It should be large to avoid dropouts.

Typical usage to start logging immediately:
$ logger start -e -t

Or if already running:
$ logger on
)DESCR_STR");

	PRINT_MODULE_USAGE_NAME("logger", "system");
	PRINT_MODULE_USAGE_COMMAND("start");
	PRINT_MODULE_USAGE_PARAM_STRING('m', "all", "file|mavlink|all", "Backend mode", true);
	PRINT_MODULE_USAGE_PARAM_FLAG('e', "Enable logging right after start until disarm (otherwise only when armed)", true);
	PRINT_MODULE_USAGE_PARAM_FLAG('f', "Log until shutdown (implies -e)", true);
	PRINT_MODULE_USAGE_PARAM_FLAG('t', "Use date/time for naming log directories and files", true);
	PRINT_MODULE_USAGE_PARAM_INT('r', 280, 0, 8000, "Log rate in Hz, 0 means unlimited rate", true);
	PRINT_MODULE_USAGE_PARAM_INT('b', 12, 4, 10000, "Log buffer size in KiB", true);
	PRINT_MODULE_USAGE_PARAM_INT('q', 14, 1, 100, "uORB queue size for mavlink mode", true);
	PRINT_MODULE_USAGE_PARAM_STRING('p', nullptr, "<topic_name>",
					 "Poll on a topic instead of running with fixed rate (Log rate and topic intervals are ignored if this is set)", true);
	PRINT_MODULE_USAGE_COMMAND_DESCR("on", "start logging now, override arming (logger must be running)");
	PRINT_MODULE_USAGE_COMMAND_DESCR("off", "stop logging now, override arming (logger must be running)");
	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();

	return 0;
}

int Logger::task_spawn(int argc, char *argv[])
{
	_task_id = px4_task_spawn_cmd("logger",
				      SCHED_DEFAULT,
				      SCHED_PRIORITY_MAX - 10,
				      3600,
				      (px4_main_t)&run_trampoline,
				      (char *const *)argv);

	if (_task_id < 0) {
		_task_id = -1;
		return -errno;
	}

	return 0;
}

int Logger::print_status()
{
	PX4_INFO("Running in mode: %s", configured_backend_mode());

	if (_writer.is_started(LogWriter::BackendFile)) {
		PX4_INFO("File Logging Running");
		print_statistics();
	}

	if (_writer.is_started(LogWriter::BackendMavlink)) {
		PX4_INFO("Mavlink Logging Running");
	}

	return 0;
}

void Logger::print_statistics()
{
	if (!_writer.is_started(LogWriter::BackendFile)) { //currently only statistics for file logging
		return;
	}

	/* this is only for the file backend */
	float kibibytes = _writer.get_total_written_file() / 1024.0f;
	float mebibytes = kibibytes / 1024.0f;
	float seconds = ((float)(hrt_absolute_time() - _start_time_file)) / 1000000.0f;

	PX4_INFO("Log file: %s/%s", _log_dir, _log_file_name);
	PX4_INFO("Wrote %4.2f MiB (avg %5.2f KiB/s)", (double)mebibytes, (double)(kibibytes / seconds));
	PX4_INFO("Since last status: dropouts: %zu (max len: %.3f s), max used buffer: %zu / %zu B",
		 _write_dropouts, (double)_max_dropout_duration, _high_water, _writer.get_buffer_size_file());
	_high_water = 0;
	_write_dropouts = 0;
	_max_dropout_duration = 0.f;
}

Logger *Logger::instanciate(int argc, char *argv[])
{
	uint32_t log_interval = 3500;
	int log_buffer_size = 12 * 1024;
	bool log_on_start = false;
	bool log_until_shutdown = false;
	bool error_flag = false;
	bool log_name_timestamp = false;
	unsigned int queue_size = 14; //TODO: we might be able to reduce this if mavlink polled on the topic and/or
	// topic sizes get reduced
	LogWriter::Backend backend = LogWriter::BackendAll;
	const char *poll_topic = nullptr;

	int myoptind = 1;
	int ch;
	const char *myoptarg = nullptr;

	while ((ch = px4_getopt(argc, argv, "r:b:etfm:q:p:", &myoptind, &myoptarg)) != EOF) {
		switch (ch) {
		case 'r': {
				unsigned long r = strtoul(myoptarg, nullptr, 10);

				if (r <= 0) {
					r = 1e6;
				}

				log_interval = 1e6 / r;
			}
			break;

		case 'e':
			log_on_start = true;
			break;

		case 'b': {
				unsigned long s = strtoul(myoptarg, nullptr, 10);

				if (s < 1) {
					s = 1;
				}

				log_buffer_size = 1024 * s;
			}
			break;

		case 't':
			log_name_timestamp = true;
			break;

		case 'f':
			log_on_start = true;
			log_until_shutdown = true;
			break;

		case 'm':
			if (!strcmp(myoptarg, "file")) {
				backend = LogWriter::BackendFile;

			} else if (!strcmp(myoptarg, "mavlink")) {
				backend = LogWriter::BackendMavlink;

			} else if (!strcmp(myoptarg, "all")) {
				backend = LogWriter::BackendAll;

			} else {
				PX4_ERR("unknown mode: %s", myoptarg);
				error_flag = true;
			}

			break;

		case 'p':
			poll_topic = myoptarg;
			break;

		case 'q':
			queue_size = strtoul(myoptarg, nullptr, 10);

			if (queue_size == 0) {
				queue_size = 1;
			}

			break;

		case '?':
			error_flag = true;
			break;

		default:
			PX4_WARN("unrecognized flag");
			error_flag = true;
			break;
		}
	}

	if (error_flag) {
		return nullptr;
	}

	Logger *logger = new Logger(backend, log_buffer_size, log_interval, poll_topic, log_on_start,
				    log_until_shutdown, log_name_timestamp, queue_size);

#if defined(DBGPRINT) && defined(__PX4_NUTTX)
	struct mallinfo alloc_info = mallinfo();
	PX4_INFO("largest free chunk: %d bytes", alloc_info.mxordblk);
	PX4_INFO("remaining free heap: %d bytes", alloc_info.fordblks);
#endif /* DBGPRINT */

	if (logger == nullptr) {
		PX4_ERR("alloc failed");

	} else {
#ifndef __PX4_NUTTX
		//check for replay mode
		const char *logfile = getenv(px4::replay::ENV_FILENAME);

		if (logfile) {
			logger->setReplayFile(logfile);
		}

#endif /* __PX4_NUTTX */

	}

	return logger;
}


Logger::Logger(LogWriter::Backend backend, size_t buffer_size, uint32_t log_interval, const char *poll_topic_name,
	       bool log_on_start, bool log_until_shutdown, bool log_name_timestamp, unsigned int queue_size) :
	_arm_override(false),
	_log_on_start(log_on_start),
	_log_until_shutdown(log_until_shutdown),
	_log_name_timestamp(log_name_timestamp),
	_writer(backend, buffer_size, queue_size),
	_log_interval(log_interval)
{
	_log_utc_offset = param_find("SDLOG_UTC_OFFSET");
	_log_dirs_max = param_find("SDLOG_DIRS_MAX");
	_sdlog_profile_handle = param_find("SDLOG_PROFILE");

	if (poll_topic_name) {
		const orb_metadata **topics = orb_get_topics();

		for (size_t i = 0; i < orb_topics_count(); i++) {
			if (strcmp(poll_topic_name, topics[i]->o_name) == 0) {
				_polling_topic_meta = topics[i];
				break;
			}
		}

		if (!_polling_topic_meta) {
			PX4_ERR("Failed to find topic %s", poll_topic_name);
		}
	}
}

Logger::~Logger()
{
	if (_replay_file_name) {
		free(_replay_file_name);
	}

	if (_msg_buffer) {
		delete[](_msg_buffer);
	}
}

bool Logger::request_stop_static()
{
	if (is_running()) {
		get_instance()->request_stop();
		return false;
	}

	return true;
}

int Logger::add_topic(const orb_metadata *topic)
{
	int fd = -1;
	size_t fields_len = strlen(topic->o_fields) + strlen(topic->o_name) + 1; //1 for ':'

	if (fields_len > sizeof(ulog_message_format_s::format)) {
		PX4_WARN("skip topic %s, format string is too large: %zu (max is %zu)", topic->o_name, fields_len,
			 sizeof(ulog_message_format_s::format));

		return -1;
	}

	fd = orb_subscribe(topic);

	if (fd < 0) {
		PX4_WARN("logger: %s subscribe failed (%i)", topic->o_name, errno);
		return -1;
	}

	if (!_subscriptions.push_back(LoggerSubscription(fd, topic))) {
		PX4_WARN("logger: failed to add topic. Too many subscriptions");
		orb_unsubscribe(fd);
		fd = -1;
	}

	return fd;
}

int Logger::add_topic(const char *name, unsigned interval = 0)
{
	const orb_metadata **topics = orb_get_topics();
	int fd = -1;

	for (size_t i = 0; i < orb_topics_count(); i++) {
		if (strcmp(name, topics[i]->o_name) == 0) {
			bool already_added = false;

			// check if already added: if so, only update the interval
			for (size_t j = 0; j < _subscriptions.size(); ++j) {
				if (_subscriptions[j].metadata == topics[i]) {
					PX4_DEBUG("logging topic %s, interval: %i, already added, only setting interval",
						  topics[i]->o_name, interval);
					fd = _subscriptions[j].fd[0];
					already_added = true;
					break;
				}
			}

			if (!already_added) {
				fd = add_topic(topics[i]);
				PX4_DEBUG("logging topic: %s, interval: %i", topics[i]->o_name, interval);
				break;
			}
		}
	}

	// if we poll on a topic, we don't set the interval and let the polled topic define the maximum interval
	if (!_polling_topic_meta && fd >= 0) {
		orb_set_interval(fd, interval);
	}

	return fd;
}

bool Logger::copy_if_updated_multi(LoggerSubscription &sub, int multi_instance, void *buffer, bool try_to_subscribe)
{
	bool updated = false;
	int &handle = sub.fd[multi_instance];

	if (handle < 0 && try_to_subscribe) {

		if (OK == orb_exists(sub.metadata, multi_instance)) {
			handle = orb_subscribe_multi(sub.metadata, multi_instance);

			//PX4_INFO("subscribed to instance %d of topic %s", multi_instance, sub.metadata->o_name);

			/* copy first data */
			if (handle >= 0) {
				write_add_logged_msg(sub, multi_instance);

				/* set to the same interval as the first instance */
				unsigned int interval;

				if (orb_get_interval(sub.fd[0], &interval) == 0 && interval > 0) {
					orb_set_interval(handle, interval);
				}

				/* It can happen that orb_exists returns true, even if there is no publisher (but another subscriber).
				 * We catch this here, because orb_copy will fail in this case. */
				if (orb_copy(sub.metadata, handle, buffer) == PX4_OK) {
					updated = true;
				}
			}
		}

	} else if (handle >= 0) {
		orb_check(handle, &updated);

		if (updated) {
			orb_copy(sub.metadata, handle, buffer);
		}
	}

	return updated;
}

void Logger::add_common_topics()
{
#ifdef CONFIG_ARCH_BOARD_SITL
	add_topic("vehicle_attitude_groundtruth", 10);
	add_topic("vehicle_global_position_groundtruth", 100);
	add_topic("vehicle_local_position_groundtruth", 100);
#endif

	// Note: try to avoid setting the interval where possible, as it increases RAM usage
	add_topic("actuator_controls_0", 100);
	add_topic("actuator_controls_1", 100);
	add_topic("actuator_outputs", 100);
	add_topic("airspeed");
	add_topic("att_pos_mocap", 50);
	add_topic("battery_status", 300);
	add_topic("camera_capture");
	add_topic("camera_trigger");
	add_topic("commander_state", 100);
	add_topic("control_state", 100);
	add_topic("cpuload");
	add_topic("differential_pressure", 50);
	add_topic("distance_sensor");
	add_topic("ekf2_innovations", 50);
	add_topic("esc_status", 250);
	add_topic("estimator_status", 200); //this one is large
	add_topic("gps_dump"); //this will only be published if gps_dump_comm is set
	add_topic("input_rc", 100);
	add_topic("optical_flow", 50);
	add_topic("position_setpoint_triplet", 200);
	add_topic("rc_channels", 100);
	add_topic("satellite_info");
	add_topic("sensor_combined", 100);
	add_topic("sensor_preflight", 50);
	add_topic("system_power", 300);
	add_topic("task_stack_info");
	add_topic("tecs_status", 20);
	add_topic("telemetry_status");
	add_topic("vehicle_attitude", 30);
	add_topic("vehicle_attitude_setpoint", 30);
	add_topic("vehicle_command");
	add_topic("vehicle_global_position", 200);
	add_topic("vehicle_gps_position");
	add_topic("vehicle_land_detected");
	add_topic("vehicle_local_position", 100);
	add_topic("vehicle_local_position_setpoint", 100);
	add_topic("vehicle_rates_setpoint", 30);
	add_topic("vehicle_status");
	add_topic("vehicle_vision_attitude");
	add_topic("vehicle_vision_position");
	add_topic("vtol_vehicle_status", 100);
	add_topic("wind_estimate", 100);
}

void Logger::add_estimator_replay_topics()
{
	// for estimator replay (need to be at full rate)
	add_topic("ekf2_timestamps");
	add_topic("sensor_combined");
}

void Logger::add_thermal_calibration_topics()
{
	// Note: try to avoid setting the interval where possible, as it increases RAM usage
	add_topic("sensor_accel", 100);
	add_topic("sensor_baro", 100);
	add_topic("sensor_gyro", 100);
}

void Logger::add_system_identification_topics()
{
	// for system id need to log imu and controls at full rate
	add_topic("actuator_controls_0");
	add_topic("actuator_controls_1");
	add_topic("sensor_combined");
}

int Logger::add_topics_from_file(const char *fname)
{
	FILE		*fp;
	char		line[80];
	char		topic_name[80];
	unsigned	interval;
	int			ntopics = 0;

	/* open the topic list file */
	fp = fopen(fname, "r");

	if (fp == nullptr) {
		return -1;
	}

	/* call add_topic for each topic line in the file */
	for (;;) {

		/* get a line, bail on error/EOF */
		line[0] = '\0';

		if (fgets(line, sizeof(line), fp) == nullptr) {
			break;
		}

		/* skip comment lines */
		if ((strlen(line) < 2) || (line[0] == '#')) {
			continue;
		}

		// read line with format: <topic_name>[, <interval>]
		interval = 0;
		int nfields = sscanf(line, "%s %u", topic_name, &interval);

		if (nfields > 0) {
			int name_len = strlen(topic_name);

			if (name_len > 0 && topic_name[name_len - 1] == ',') {
				topic_name[name_len - 1] = '\0';
			}

			/* add topic with specified interval */
			if (add_topic(topic_name, interval) >= 0) {
				ntopics++;

			} else {
				PX4_ERR("Failed to add topic %s", topic_name);
			}
		}
	}

	fclose(fp);
	return ntopics;
}

const char *Logger::configured_backend_mode() const
{
	switch (_writer.backend()) {
	case LogWriter::BackendFile: return "file";

	case LogWriter::BackendMavlink: return "mavlink";

	case LogWriter::BackendAll: return "all";

	default: return "several";
	}
}

void Logger::run()
{
#ifdef DBGPRINT
	struct mallinfo alloc_info = {};
#endif /* DBGPRINT */

	PX4_INFO("logger started (mode=%s)", configured_backend_mode());

	if (_writer.backend() & LogWriter::BackendFile) {
		int mkdir_ret = mkdir(LOG_ROOT, S_IRWXU | S_IRWXG | S_IRWXO);

		if (mkdir_ret == 0) {
			PX4_INFO("log root dir created: %s", LOG_ROOT);

		} else if (errno != EEXIST) {
			PX4_ERR("failed creating log root dir: %s", LOG_ROOT);

			if ((_writer.backend() & ~LogWriter::BackendFile) == 0) {
				return;
			}
		}

		if (check_free_space() == 1) {
			return;
		}
	}

	int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
	uORB::Subscription<parameter_update_s> parameter_update_sub(ORB_ID(parameter_update));
	int log_message_sub = orb_subscribe(ORB_ID(log_message));
	orb_set_interval(log_message_sub, 20);

	int ntopics = add_topics_from_file(PX4_ROOTFSDIR "/fs/microsd/etc/logging/logger_topics.txt");

	if (ntopics > 0) {
		PX4_INFO("logging %d topics from logger_topics.txt", ntopics);

	} else {

		// get the logging profile
		SDLogProfile sdlog_profile = SDLogProfile::DEFAULT;

		if (_sdlog_profile_handle != PARAM_INVALID) {
			param_get(_sdlog_profile_handle, &sdlog_profile);
		}

		// load appropriate topics for profile
		if (sdlog_profile == SDLogProfile::DEFAULT) {
			add_common_topics();
			add_estimator_replay_topics();

		} else if (sdlog_profile == SDLogProfile::THERMAL_CALIBRATION) {
			add_thermal_calibration_topics();

		} else if (sdlog_profile == SDLogProfile::SYSTEM_IDENTIFICATION) {
			add_common_topics();
			add_system_identification_topics();

		} else {
			add_common_topics();
			add_estimator_replay_topics();
		}
	}

	int vehicle_command_sub = -1;
	orb_advert_t vehicle_command_ack_pub = nullptr;

	if (_writer.backend() & LogWriter::BackendMavlink) {
		vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
	}

	//all topics added. Get required message buffer size
	int max_msg_size = 0, ret;

	for (const auto &subscription : _subscriptions) {
		//use o_size, because that's what orb_copy will use
		if (subscription.metadata->o_size > max_msg_size) {
			max_msg_size = subscription.metadata->o_size;
		}
	}

	max_msg_size += sizeof(ulog_message_data_header_s);

	if (sizeof(ulog_message_logging_s) > max_msg_size) {
		max_msg_size = sizeof(ulog_message_logging_s);
	}

	if (_polling_topic_meta && _polling_topic_meta->o_size > max_msg_size) {
		max_msg_size = _polling_topic_meta->o_size;
	}

	if (max_msg_size > _msg_buffer_len) {
		if (_msg_buffer) {
			delete[](_msg_buffer);
		}

		_msg_buffer_len = max_msg_size;
		_msg_buffer = new uint8_t[_msg_buffer_len];

		if (!_msg_buffer) {
			PX4_ERR("failed to alloc message buffer");
			return;
		}
	}


	if (!_writer.init()) {
		PX4_ERR("writer init failed");
		return;
	}

#ifdef DBGPRINT
	hrt_abstime	timer_start = 0;
	uint32_t	total_bytes = 0;
#endif /* DBGPRINT */

	px4_register_shutdown_hook(&Logger::request_stop_static);

	// we start logging immediately
	// the case where we wait with logging until vehicle is armed is handled below
	if (_log_on_start) {
		start_log_file();
	}

	/* init the update timer */
	struct hrt_call timer_call;
	memset(&timer_call, 0, sizeof(hrt_call));
	px4_sem_t timer_semaphore;
	px4_sem_init(&timer_semaphore, 0, 0);
	/* timer_semaphore use case is a signal */
	px4_sem_setprotocol(&timer_semaphore, SEM_PRIO_NONE);

	int polling_topic_sub = -1;

	if (_polling_topic_meta) {
		polling_topic_sub = orb_subscribe(_polling_topic_meta);

		if (polling_topic_sub < 0) {
			PX4_ERR("Failed to subscribe (%i)", errno);
		}

	} else {
		hrt_call_every(&timer_call, _log_interval, _log_interval, timer_callback, &timer_semaphore);
	}

	// check for new subscription data
	hrt_abstime next_subscribe_check = 0;
	int next_subscribe_topic_index = -1; // this is used to distribute the checks over time

	while (!should_exit()) {

		// Start/stop logging when system arm/disarm
		bool vehicle_status_updated;
		ret = orb_check(vehicle_status_sub, &vehicle_status_updated);

		if (ret == 0 && vehicle_status_updated) {
			vehicle_status_s vehicle_status;
			orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status);
			bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) ||
				     (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) ||
				     _arm_override;

			if (_was_armed != armed && !_log_until_shutdown) {
				_was_armed = armed;

				if (armed) {

					if (_should_stop_file_log) { // happens on quick arming after disarm
						_should_stop_file_log = false;
						stop_log_file();
					}

					start_log_file();

#ifdef DBGPRINT
					timer_start = hrt_absolute_time();
					total_bytes = 0;
#endif /* DBGPRINT */

				} else {
					// delayed stop: we measure the process loads and then stop
					initialize_load_output();
					_should_stop_file_log = true;
				}
			}
		}

		/* check for logging command from MAVLink */
		if (vehicle_command_sub != -1) {
			bool command_updated = false;
			ret = orb_check(vehicle_command_sub, &command_updated);

			if (ret == 0 && command_updated) {
				vehicle_command_s command;
				orb_copy(ORB_ID(vehicle_command), vehicle_command_sub, &command);

				if (command.command == vehicle_command_s::VEHICLE_CMD_LOGGING_START) {
					if ((int)(command.param1 + 0.5f) != 0) {
						ack_vehicle_command(vehicle_command_ack_pub, command.command,
								    vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED);

					} else if (can_start_mavlink_log()) {
						ack_vehicle_command(vehicle_command_ack_pub, command.command,
								    vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
						start_log_mavlink();

					} else {
						ack_vehicle_command(vehicle_command_ack_pub, command.command,
								    vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
					}

				} else if (command.command == vehicle_command_s::VEHICLE_CMD_LOGGING_STOP) {
					stop_log_mavlink();
					ack_vehicle_command(vehicle_command_ack_pub, command.command,
							    vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
				}
			}
		}


		if (_writer.is_started()) {

			hrt_abstime loop_time = hrt_absolute_time();

			/* check if we need to output the process load */
			if (_next_load_print != 0 && loop_time >= _next_load_print) {
				_next_load_print = 0;

				if (_should_stop_file_log) {
					_should_stop_file_log = false;
					write_load_output(false);
					stop_log_file();
					continue; // skip to next loop iteration

				} else {
					write_load_output(true);
				}
			}

			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()) {
				parameter_update_sub.update();
				write_changed_parameters();
			}

			/* wait for lock on log buffer */
			_writer.lock();

			int sub_idx = 0;

			for (LoggerSubscription &sub : _subscriptions) {
				/* each message consists of a header followed by an orb data object
				 */
				size_t msg_size = sizeof(ulog_message_data_header_s) + sub.metadata->o_size_no_padding;

				/* if this topic has been updated, copy the new data into the message buffer
				 * and write a message to the log
				 */
				for (uint8_t instance = 0; instance < ORB_MULTI_MAX_INSTANCES; instance++) {
					if (copy_if_updated_multi(sub, instance, _msg_buffer + sizeof(ulog_message_data_header_s),
								  sub_idx == next_subscribe_topic_index)) {

						uint16_t write_msg_size = static_cast<uint16_t>(msg_size - ULOG_MSG_HEADER_LEN);
						//write one byte after another (necessary because of alignment)
						_msg_buffer[0] = (uint8_t)write_msg_size;
						_msg_buffer[1] = (uint8_t)(write_msg_size >> 8);
						_msg_buffer[2] = static_cast<uint8_t>(ULogMessageType::DATA);
						uint16_t write_msg_id = sub.msg_ids[instance];
						_msg_buffer[3] = (uint8_t)write_msg_id;
						_msg_buffer[4] = (uint8_t)(write_msg_id >> 8);

						//PX4_INFO("topic: %s, size = %zu, out_size = %zu", sub.metadata->o_name, sub.metadata->o_size, msg_size);

						if (write_message(_msg_buffer, msg_size)) {

#ifdef DBGPRINT
							total_bytes += msg_size;
#endif /* DBGPRINT */

							data_written = true;

						} else {
							break;	// Write buffer overflow, skip this record
						}
					}
				}

				++sub_idx;
			}

			//check for new logging message(s)
			bool log_message_updated = false;
			ret = orb_check(log_message_sub, &log_message_updated);

			if (ret == 0 && log_message_updated) {
				log_message_s log_message;
				orb_copy(ORB_ID(log_message), log_message_sub, &log_message);
				const char *message = (const char *)log_message.text;
				int message_len = strlen(message);

				if (message_len > 0) {
					uint16_t write_msg_size = sizeof(ulog_message_logging_s) - sizeof(ulog_message_logging_s::message)
								  - ULOG_MSG_HEADER_LEN + message_len;
					_msg_buffer[0] = (uint8_t)write_msg_size;
					_msg_buffer[1] = (uint8_t)(write_msg_size >> 8);
					_msg_buffer[2] = static_cast<uint8_t>(ULogMessageType::LOGGING);
					_msg_buffer[3] = log_message.severity + '0';
					memcpy(_msg_buffer + 4, &log_message.timestamp, sizeof(ulog_message_logging_s::timestamp));
					strncpy((char *)(_msg_buffer + 12), message, sizeof(ulog_message_logging_s::message));

					write_message(_msg_buffer, write_msg_size + ULOG_MSG_HEADER_LEN);
				}
			}

			if (!_dropout_start && _writer.get_buffer_fill_count_file() > _high_water) {
				_high_water = _writer.get_buffer_fill_count_file();
			}

			/* release the log buffer */
			_writer.unlock();

			/* notify the writer thread if data is available */
			if (data_written) {
				_writer.notify();
			}

			/* subscription update */
			if (next_subscribe_topic_index != -1) {
				if (++next_subscribe_topic_index >= _subscriptions.size()) {
					next_subscribe_topic_index = -1;
					next_subscribe_check = loop_time + TRY_SUBSCRIBE_INTERVAL;
				}

			} else if (loop_time > next_subscribe_check) {
				next_subscribe_topic_index = 0;
			}

#ifdef DBGPRINT
			double deltat = (double)(hrt_absolute_time() - timer_start)  * 1e-6;

			if (deltat > 4.0) {
				alloc_info = mallinfo();
				double throughput = total_bytes / deltat;
				PX4_INFO("%8.1f kB/s, %zu highWater,  %d dropouts, %5.3f sec max, free heap: %d",
					 throughput / 1.e3, _high_water, _write_dropouts, (double)_max_dropout_duration,
					 alloc_info.fordblks);

				_high_water = 0;
				_max_dropout_duration = 0.f;
				total_bytes = 0;
				timer_start = hrt_absolute_time();
			}

#endif /* DBGPRINT */

		}

		// wait for next loop iteration...
		if (polling_topic_sub >= 0) {
			px4_pollfd_struct_t fds[1];
			fds[0].fd = polling_topic_sub;
			fds[0].events = POLLIN;
			int pret = px4_poll(fds, 1, 1000);

			if (pret < 0) {
				PX4_ERR("poll failed (%i)", pret);

			} else if (pret != 0) {
				if (fds[0].revents & POLLIN) {
					// need to to an orb_copy so that poll will not return immediately
					orb_copy(_polling_topic_meta, polling_topic_sub, _msg_buffer);
				}
			}

		} else {
			/*
			 * We wait on the semaphore, which periodically gets updated by a high-resolution timer.
			 * The simpler alternative would be:
			 *   usleep(max(300, _log_interval - elapsed_time_since_loop_start));
			 * And on linux this is quite accurate as well, but under NuttX it is not accurate,
			 * because usleep() has only a granularity of CONFIG_MSEC_PER_TICK (=1ms).
			 */
			while (px4_sem_wait(&timer_semaphore) != 0);
		}
	}

	stop_log_file();

	hrt_cancel(&timer_call);
	px4_sem_destroy(&timer_semaphore);

	// stop the writer thread
	_writer.thread_stop();

	//unsubscribe
	for (LoggerSubscription &sub : _subscriptions) {
		for (uint8_t instance = 0; instance < ORB_MULTI_MAX_INSTANCES; instance++) {
			if (sub.fd[instance] != -1) {
				orb_unsubscribe(sub.fd[instance]);
				sub.fd[instance] = -1;
			}
		}
	}

	if (polling_topic_sub >= 0) {
		orb_unsubscribe(polling_topic_sub);
	}

	if (_mavlink_log_pub) {
		orb_unadvertise(_mavlink_log_pub);
		_mavlink_log_pub = nullptr;
	}

	if (vehicle_command_ack_pub) {
		orb_unadvertise(vehicle_command_ack_pub);
	}

	if (vehicle_command_sub != -1) {
		orb_unsubscribe(vehicle_command_sub);
	}

	px4_unregister_shutdown_hook(&Logger::request_stop_static);
}

bool Logger::write_message(void *ptr, size_t size)
{
	if (_writer.write_message(ptr, size, _dropout_start) != -1) {

		if (_dropout_start) {
			float dropout_duration = (float)(hrt_elapsed_time(&_dropout_start) / 1000) / 1.e3f;

			if (dropout_duration > _max_dropout_duration) {
				_max_dropout_duration = dropout_duration;
			}

			_dropout_start = 0;
		}

		return true;
	}

	if (!_dropout_start) {
		_dropout_start = hrt_absolute_time();
		++_write_dropouts;
		_high_water = 0;
	}

	return false;
}

int Logger::create_log_dir(tm *tt)
{
	/* create dir on sdcard if needed */
	int mkdir_ret;

	if (tt) {
		int n = snprintf(_log_dir, sizeof(_log_dir), "%s/", LOG_ROOT);

		if (n >= sizeof(_log_dir)) {
			PX4_ERR("log path too long");
			return -1;
		}

		strftime(_log_dir + n, sizeof(_log_dir) - n, "%Y-%m-%d", tt);
		mkdir_ret = mkdir(_log_dir, S_IRWXU | S_IRWXG | S_IRWXO);

		if (mkdir_ret != OK && errno != EEXIST) {
			PX4_ERR("failed creating new dir: %s", _log_dir);
			return -1;
		}

	} else {
		uint16_t dir_number = _sess_dir_index;

		/* look for the next dir that does not exist */
		while (!_has_log_dir) {
			/* format log dir: e.g. /fs/microsd/sess001 */
			int n = snprintf(_log_dir, sizeof(_log_dir), "%s/sess%03u", LOG_ROOT, dir_number);

			if (n >= sizeof(_log_dir)) {
				PX4_ERR("log path too long (%i)", n);
				return -1;
			}

			mkdir_ret = mkdir(_log_dir, S_IRWXU | S_IRWXG | S_IRWXO);

			if (mkdir_ret == 0) {
				PX4_DEBUG("log dir created: %s", _log_dir);
				_has_log_dir = true;

			} else if (errno != EEXIST) {
				PX4_ERR("failed creating new dir: %s (%i)", _log_dir, errno);
				return -1;
			}

			/* dir exists already */
			dir_number++;
		}

		_has_log_dir = true;
	}

	return 0;
}

bool Logger::file_exist(const char *filename)
{
	struct stat buffer;
	return stat(filename, &buffer) == 0;
}

int Logger::get_log_file_name(char *file_name, size_t file_name_size)
{
	tm tt;
	bool time_ok = false;

	if (_log_name_timestamp) {
		/* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.ulg */
		time_ok = get_log_time(&tt, false);
	}

	const char *replay_suffix = "";

	if (_replay_file_name) {
		replay_suffix = "_replayed";
	}


	if (time_ok) {
		if (create_log_dir(&tt)) {
			return -1;
		}

		char log_file_name_time[16] = "";
		strftime(log_file_name_time, sizeof(log_file_name_time), "%H_%M_%S", &tt);
		snprintf(_log_file_name, sizeof(_log_file_name), "%s%s.ulg", log_file_name_time, replay_suffix);
		snprintf(file_name, file_name_size, "%s/%s", _log_dir, _log_file_name);

	} else {
		if (create_log_dir(nullptr)) {
			return -1;
		}

		uint16_t file_number = 1; // start with file log001

		/* look for the next file that does not exist */
		while (file_number <= MAX_NO_LOGFILE) {
			/* format log file path: e.g. /fs/microsd/sess001/log001.ulg */
			snprintf(_log_file_name, sizeof(_log_file_name), "log%03u%s.ulg", file_number, replay_suffix);
			snprintf(file_name, file_name_size, "%s/%s", _log_dir, _log_file_name);

			if (!file_exist(file_name)) {
				break;
			}

			file_number++;
		}

		if (file_number > MAX_NO_LOGFILE) {
			/* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */
			return -1;
		}
	}


	return 0;
}

void Logger::setReplayFile(const char *file_name)
{
	if (_replay_file_name) {
		free(_replay_file_name);
	}

	_replay_file_name = strdup(file_name);
}

bool Logger::get_log_time(struct tm *tt, bool boot_time)
{
	int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));

	if (vehicle_gps_position_sub < 0) {
		return false;
	}

	/* Get the latest GPS publication */
	vehicle_gps_position_s gps_pos;
	time_t utc_time_sec;
	bool use_clock_time = true;

	if (orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps_pos) == 0) {
		utc_time_sec = gps_pos.time_utc_usec / 1e6;

		if (gps_pos.fix_type >= 2 && utc_time_sec >= GPS_EPOCH_SECS) {
			use_clock_time = false;
		}
	}

	orb_unsubscribe(vehicle_gps_position_sub);

	if (use_clock_time) {
		/* take clock time if there's no fix (yet) */
		struct timespec ts;
		px4_clock_gettime(CLOCK_REALTIME, &ts);
		utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9);

		if (utc_time_sec < GPS_EPOCH_SECS) {
			return false;
		}
	}

	/* strip the time elapsed since boot */
	if (boot_time) {
		utc_time_sec -= hrt_absolute_time() / 1e6;
	}

	int32_t utc_offset = 0;

	if (_log_utc_offset != PARAM_INVALID) {
		param_get(_log_utc_offset, &utc_offset);
	}

	/* apply utc offset */
	utc_time_sec += utc_offset * 60;

	return gmtime_r(&utc_time_sec, tt) != nullptr;
}

void Logger::start_log_file()
{
	if (_writer.is_started(LogWriter::BackendFile) || (_writer.backend() & LogWriter::BackendFile) == 0) {
		return;
	}

	PX4_INFO("Start file log");

	char file_name[LOG_DIR_LEN] = "";

	if (get_log_file_name(file_name, sizeof(file_name))) {
		PX4_ERR("logger: failed to get log file name");
		return;
	}

	/* print logging path, important to find log file later */
	mavlink_log_info(&_mavlink_log_pub, "[logger] file: %s", file_name);

	_writer.start_log_file(file_name);
	_writer.select_write_backend(LogWriter::BackendFile);
	_writer.set_need_reliable_transfer(true);
	write_header();
	write_version();
	write_formats();
	write_parameters();
	write_perf_data(true);
	write_all_add_logged_msg();
	_writer.set_need_reliable_transfer(false);
	_writer.unselect_write_backend();
	_writer.notify();

	/* reset performance counters to get in-flight min and max values in post flight log */
	perf_reset_all();

	_start_time_file = hrt_absolute_time();

	initialize_load_output();
}

void Logger::stop_log_file()
{
	if (!_writer.is_started(LogWriter::BackendFile)) {
		return;
	}

	_writer.set_need_reliable_transfer(true);
	write_perf_data(false);
	_writer.set_need_reliable_transfer(false);
	_writer.stop_log_file();
}

void Logger::start_log_mavlink()
{
	if (!can_start_mavlink_log()) {
		return;
	}

	PX4_INFO("Start mavlink log");

	_writer.start_log_mavlink();
	_writer.select_write_backend(LogWriter::BackendMavlink);
	_writer.set_need_reliable_transfer(true);
	write_header();
	write_version();
	write_formats();
	write_parameters();
	write_perf_data(true);
	write_all_add_logged_msg();
	_writer.set_need_reliable_transfer(false);
	_writer.unselect_write_backend();
	_writer.notify();

	initialize_load_output();
}

void Logger::stop_log_mavlink()
{
	// don't write perf data since a client does not expect more data after a stop command
	PX4_INFO("Stop mavlink log");
	_writer.stop_log_mavlink();
}

struct perf_callback_data_t {
	Logger *logger;
	int counter;
	bool preflight;
	char *buffer;
};

void Logger::perf_iterate_callback(perf_counter_t handle, void *user)
{
	perf_callback_data_t *callback_data = (perf_callback_data_t *)user;
	const int buffer_length = 256;
	char buffer[buffer_length];
	const char *perf_name;

	perf_print_counter_buffer(buffer, buffer_length, handle);

	if (callback_data->preflight) {
		perf_name = "perf_counter_preflight";

	} else {
		perf_name = "perf_counter_postflight";
	}

	callback_data->logger->write_info_multiple(perf_name, buffer, callback_data->counter != 0);
	++callback_data->counter;
}

void Logger::write_perf_data(bool preflight)
{
	perf_callback_data_t callback_data;
	callback_data.logger = this;
	callback_data.counter = 0;
	callback_data.preflight = preflight;

	// write the perf counters
	perf_iterate_all(perf_iterate_callback, &callback_data);
}


void Logger::print_load_callback(void *user)
{
	perf_callback_data_t *callback_data = (perf_callback_data_t *)user;
	const char *perf_name;

	if (!callback_data->buffer) {
		return;
	}

	if (callback_data->preflight) {
		perf_name = "perf_top_preflight";

	} else {
		perf_name = "perf_top_postflight";
	}

	callback_data->logger->write_info_multiple(perf_name, callback_data->buffer, callback_data->counter != 0);
	++callback_data->counter;
}

void Logger::initialize_load_output()
{
	perf_callback_data_t callback_data;
	callback_data.logger = this;
	callback_data.counter = 0;
	callback_data.buffer = nullptr;
	char buffer[140];
	hrt_abstime curr_time = hrt_absolute_time();
	init_print_load_s(curr_time, &_load);
	// this will not yet print anything
	print_load_buffer(curr_time, buffer, sizeof(buffer), print_load_callback, &callback_data, &_load);
	_next_load_print = curr_time + 1000000;
}

void Logger::write_load_output(bool preflight)
{
	perf_callback_data_t callback_data;
	char buffer[140];
	callback_data.logger = this;
	callback_data.counter = 0;
	callback_data.buffer = buffer;
	callback_data.preflight = preflight;
	hrt_abstime curr_time = hrt_absolute_time();
	_writer.set_need_reliable_transfer(true);
	// TODO: maybe we should restrict the output to a selected backend (eg. when file logging is running
	// and mavlink log is started, this will be added to the file as well)
	print_load_buffer(curr_time, buffer, sizeof(buffer), print_load_callback, &callback_data, &_load);
	_writer.set_need_reliable_transfer(false);
}

void Logger::write_formats()
{
	_writer.lock();
	ulog_message_format_s msg;
	const orb_metadata **topics = orb_get_topics();

	//write all known formats
	for (size_t i = 0; i < orb_topics_count(); i++) {
		int format_len = snprintf(msg.format, sizeof(msg.format), "%s:%s", topics[i]->o_name, topics[i]->o_fields);
		size_t msg_size = sizeof(msg) - sizeof(msg.format) + format_len;
		msg.msg_size = msg_size - ULOG_MSG_HEADER_LEN;

		write_message(&msg, msg_size);
	}

	_writer.unlock();
}

void Logger::write_all_add_logged_msg()
{
	_writer.lock();

	for (LoggerSubscription &sub : _subscriptions) {
		for (int instance = 0; instance < ORB_MULTI_MAX_INSTANCES; ++instance) {
			if (sub.fd[instance] >= 0) {
				write_add_logged_msg(sub, instance);
			}
		}
	}

	_writer.unlock();
}

void Logger::write_add_logged_msg(LoggerSubscription &subscription, int instance)
{
	ulog_message_add_logged_s msg;

	if (subscription.msg_ids[instance] == (uint16_t) - 1) {
		subscription.msg_ids[instance] = _next_topic_id++;
	}

	msg.msg_id = subscription.msg_ids[instance];
	msg.multi_id = instance;

	int message_name_len = strlen(subscription.metadata->o_name);

	memcpy(msg.message_name, subscription.metadata->o_name, message_name_len);

	size_t msg_size = sizeof(msg) - sizeof(msg.message_name) + message_name_len;
	msg.msg_size = msg_size - ULOG_MSG_HEADER_LEN;

	bool prev_reliable = _writer.need_reliable_transfer();
	_writer.set_need_reliable_transfer(true);
	write_message(&msg, msg_size);
	_writer.set_need_reliable_transfer(prev_reliable);
}

/* write info message */
void Logger::write_info(const char *name, const char *value)
{
	_writer.lock();
	ulog_message_info_header_s msg;
	uint8_t *buffer = reinterpret_cast<uint8_t *>(&msg);
	msg.msg_type = static_cast<uint8_t>(ULogMessageType::INFO);

	/* construct format key (type and name) */
	size_t vlen = strlen(value);
	msg.key_len = snprintf(msg.key, sizeof(msg.key), "char[%zu] %s", vlen, name);
	size_t msg_size = sizeof(msg) - sizeof(msg.key) + msg.key_len;

	/* copy string value directly to buffer */
	if (vlen < (sizeof(msg) - msg_size)) {
		memcpy(&buffer[msg_size], value, vlen);
		msg_size += vlen;

		msg.msg_size = msg_size - ULOG_MSG_HEADER_LEN;

		write_message(buffer, msg_size);
	}

	_writer.unlock();
}

void Logger::write_info_multiple(const char *name, const char *value, bool is_continued)
{
	_writer.lock();
	ulog_message_info_multiple_header_s msg;
	uint8_t *buffer = reinterpret_cast<uint8_t *>(&msg);
	msg.msg_type = static_cast<uint8_t>(ULogMessageType::INFO_MULTIPLE);
	msg.is_continued = is_continued;

	/* construct format key (type and name) */
	size_t vlen = strlen(value);
	msg.key_len = snprintf(msg.key, sizeof(msg.key), "char[%zu] %s", vlen, name);
	size_t msg_size = sizeof(msg) - sizeof(msg.key) + msg.key_len;

	/* copy string value directly to buffer */
	if (vlen < (sizeof(msg) - msg_size)) {
		memcpy(&buffer[msg_size], value, vlen);
		msg_size += vlen;

		msg.msg_size = msg_size - ULOG_MSG_HEADER_LEN;

		write_message(buffer, msg_size);
	}

	_writer.unlock();
}

void Logger::write_info(const char *name, int32_t value)
{
	write_info_template<int32_t>(name, value, "int32_t");
}

void Logger::write_info(const char *name, uint32_t value)
{
	write_info_template<uint32_t>(name, value, "uint32_t");
}


template<typename T>
void Logger::write_info_template(const char *name, T value, const char *type_str)
{
	_writer.lock();
	ulog_message_info_header_s msg;
	uint8_t *buffer = reinterpret_cast<uint8_t *>(&msg);
	msg.msg_type = static_cast<uint8_t>(ULogMessageType::INFO);

	/* construct format key (type and name) */
	msg.key_len = snprintf(msg.key, sizeof(msg.key), "%s %s", type_str, name);
	size_t msg_size = sizeof(msg) - sizeof(msg.key) + msg.key_len;

	/* copy string value directly to buffer */
	memcpy(&buffer[msg_size], &value, sizeof(T));
	msg_size += sizeof(T);

	msg.msg_size = msg_size - ULOG_MSG_HEADER_LEN;

	write_message(buffer, msg_size);

	_writer.unlock();
}

void Logger::write_header()
{
	ulog_file_header_s header;
	header.magic[0] = 'U';
	header.magic[1] = 'L';
	header.magic[2] = 'o';
	header.magic[3] = 'g';
	header.magic[4] = 0x01;
	header.magic[5] = 0x12;
	header.magic[6] = 0x35;
	header.magic[7] = 0x01; //file version 1
	header.timestamp = hrt_absolute_time();
	_writer.lock();
	write_message(&header, sizeof(header));

	// write the Flags message: this MUST be written right after the ulog header
	ulog_message_flag_bits_s flag_bits;

	memset(&flag_bits, 0, sizeof(flag_bits));
	flag_bits.msg_size = sizeof(flag_bits) - ULOG_MSG_HEADER_LEN;
	flag_bits.msg_type = static_cast<uint8_t>(ULogMessageType::FLAG_BITS);

	write_message(&flag_bits, sizeof(flag_bits));

	_writer.unlock();
}

/* write version info messages */
void Logger::write_version()
{
	write_info("ver_sw", px4_firmware_version_string());
	write_info("ver_sw_release", px4_firmware_version());
	write_info("ver_hw", px4_board_name());
	write_info("sys_name", "PX4");
	write_info("sys_os_name", px4_os_name());
	const char *os_version = px4_os_version_string();

	const char *git_branch = px4_firmware_git_branch();

	if (git_branch && git_branch[0]) {
		write_info("ver_sw_branch", git_branch);
	}

	if (os_version) {
		write_info("sys_os_ver", os_version);
	}

	write_info("sys_os_ver_release", px4_os_version());
	write_info("sys_toolchain", px4_toolchain_name());
	write_info("sys_toolchain_ver", px4_toolchain_version());

	char revision = 'U';
	const char *chip_name = nullptr;

	if (board_mcu_version(&revision, &chip_name, nullptr) >= 0) {
		char mcu_ver[64];
		snprintf(mcu_ver, sizeof(mcu_ver), "%s, rev. %c", chip_name, revision);
		write_info("sys_mcu", mcu_ver);
	}

	/* write the UUID if enabled */
	param_t write_uuid_param = param_find("SDLOG_UUID");

	if (write_uuid_param != PARAM_INVALID) {
		uint32_t write_uuid;
		param_get(write_uuid_param, &write_uuid);

		if (write_uuid == 1) {
			char uuid_string[PX4_CPU_UUID_WORD32_FORMAT_SIZE];
			board_get_uuid32_formated(uuid_string, sizeof(uuid_string), "%08X", NULL);
			write_info("sys_uuid", uuid_string);
		}
	}

	int32_t utc_offset = 0;

	if (_log_utc_offset != PARAM_INVALID) {
		param_get(_log_utc_offset, &utc_offset);
		write_info("time_ref_utc", utc_offset * 60);
	}

	if (_replay_file_name) {
		write_info("replay", _replay_file_name);
	}
}

void Logger::write_parameters()
{
	_writer.lock();
	ulog_message_parameter_header_s msg;
	uint8_t *buffer = reinterpret_cast<uint8_t *>(&msg);

	msg.msg_type = static_cast<uint8_t>(ULogMessageType::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));

		// save parameters which are valid AND used
		if (param != PARAM_INVALID) {
			/* 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 - ULOG_MSG_HEADER_LEN;

			write_message(buffer, msg_size);
		}
	} while ((param != PARAM_INVALID) && (param_idx < (int) param_count()));

	_writer.unlock();
	_writer.notify();
}

void Logger::write_changed_parameters()
{
	_writer.lock();
	ulog_message_parameter_header_s msg;
	uint8_t *buffer = reinterpret_cast<uint8_t *>(&msg);

	msg.msg_type = static_cast<uint8_t>(ULogMessageType::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)) {

			/* 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_size is now 1 (msg_type) + 2 (msg_size) + 1 (key_len) + key_len + value_size */
			msg.msg_size = msg_size - ULOG_MSG_HEADER_LEN;

			write_message(buffer, msg_size);
		}
	} while ((param != PARAM_INVALID) && (param_idx < (int) param_count()));

	_writer.unlock();
	_writer.notify();
}

int Logger::check_free_space()
{
	struct statfs statfs_buf;

	int32_t max_log_dirs_to_keep = 0;

	if (_log_dirs_max != PARAM_INVALID) {
		param_get(_log_dirs_max, &max_log_dirs_to_keep);
	}

	if (max_log_dirs_to_keep == 0) {
		max_log_dirs_to_keep = INT32_MAX;
	}

	/* remove old logs if the free space falls below a threshold */
	do {
		if (statfs(LOG_ROOT, &statfs_buf) != 0) {
			return PX4_ERROR;
		}

		DIR *dp = opendir(LOG_ROOT);

		if (dp == nullptr) {
			break; // ignore if we cannot access the log directory
		}

		struct dirent *result = nullptr;

		int num_sess = 0, num_dates = 0;

		// There are 2 directory naming schemes: sess<i> or <year>-<month>-<day>.
		// For both we find the oldest and then remove the one which has more directories.
		int year_min = 10000, month_min = 99, day_min = 99, sess_idx_min = 99999999, sess_idx_max = 0;

		while ((result = readdir(dp))) {
			int year, month, day, sess_idx;

			if (sscanf(result->d_name, "sess%d", &sess_idx) == 1) {
				++num_sess;

				if (sess_idx > sess_idx_max) {
					sess_idx_max = sess_idx;
				}

				if (sess_idx < sess_idx_min) {
					sess_idx_min = sess_idx;
				}

			} else if (sscanf(result->d_name, "%d-%d-%d", &year, &month, &day) == 3) {
				++num_dates;

				if (year < year_min) {
					year_min = year;
					month_min = month;
					day_min = day;

				} else if (year == year_min) {
					if (month < month_min) {
						month_min = month;
						day_min = day;

					} else if (month == month_min) {
						if (day < day_min) {
							day_min = day;
						}
					}
				}
			}
		}

		closedir(dp);

		_sess_dir_index = sess_idx_max + 1;


		uint64_t min_free_bytes = 300ULL * 1024ULL * 1024ULL;
		uint64_t total_bytes = statfs_buf.f_blocks * statfs_buf.f_bsize / 10;

		if (total_bytes / 10 < min_free_bytes) { // reduce the minimum if it's larger than 10% of the disk size
			min_free_bytes = total_bytes / 10;
		}

		if (num_sess + num_dates <= max_log_dirs_to_keep &&
		    statfs_buf.f_bavail >= (px4_statfs_buf_f_bavail_t)(min_free_bytes / statfs_buf.f_bsize)) {
			break; // enough free space and limit not reached
		}

		if (num_sess == 0 && num_dates == 0) {
			break; // nothing to delete
		}

		char directory_to_delete[LOG_DIR_LEN];
		int n;

		if (num_sess >= num_dates) {
			n = snprintf(directory_to_delete, sizeof(directory_to_delete), "%s/sess%03u", LOG_ROOT, sess_idx_min);

		} else {
			n = snprintf(directory_to_delete, sizeof(directory_to_delete), "%s/%04u-%02u-%02u", LOG_ROOT, year_min, month_min,
				     day_min);
		}

		if (n >= sizeof(directory_to_delete)) {
			PX4_ERR("log path too long (%i)", n);
			break;
		}

		PX4_WARN("removing log directory %s to get more space (left=%u MiB)", directory_to_delete,
			 (unsigned int)(statfs_buf.f_bavail / 1024U * statfs_buf.f_bsize / 1024U));

		if (remove_directory(directory_to_delete)) {
			PX4_ERR("Failed to delete directory");
			break;
		}

	} while (true);


	/* use a threshold of 50 MiB: if below, do not start logging */
	if (statfs_buf.f_bavail < (px4_statfs_buf_f_bavail_t)(50 * 1024 * 1024 / statfs_buf.f_bsize)) {
		mavlink_log_critical(&_mavlink_log_pub,
				     "[logger] Not logging; SD almost full: %u MiB",
				     (unsigned int)(statfs_buf.f_bavail / 1024U * statfs_buf.f_bsize / 1024U));
		return 1;
	}

	return PX4_OK;
}

int Logger::remove_directory(const char *dir)
{
	DIR *d = opendir(dir);
	size_t dir_len = strlen(dir);
	struct dirent *p;
	int ret = 0;

	if (!d) {
		return -1;
	}

	while (!ret && (p = readdir(d))) {
		int ret2 = -1;
		char *buf;
		size_t len;

		if (!strcmp(p->d_name, ".") || !strcmp(p->d_name, "..")) {
			continue;
		}

		len = dir_len + strlen(p->d_name) + 2;
		buf = new char[len];

		if (buf) {
			struct stat statbuf;

			snprintf(buf, len, "%s/%s", dir, p->d_name);

			if (!stat(buf, &statbuf)) {
				if (S_ISDIR(statbuf.st_mode)) {
					ret2 = remove_directory(buf);

				} else {
					ret2 = unlink(buf);
				}
			}

			delete[] buf;
		}

		ret = ret2;
	}

	closedir(d);

	if (!ret) {
		ret = rmdir(dir);
	}

	return ret;
}

void Logger::ack_vehicle_command(orb_advert_t &vehicle_command_ack_pub, uint16_t command, uint32_t result)
{
	vehicle_command_ack_s vehicle_command_ack;
	vehicle_command_ack.timestamp = hrt_absolute_time();
	vehicle_command_ack.command = command;
	vehicle_command_ack.result = result;

	if (vehicle_command_ack_pub == nullptr) {
		vehicle_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &vehicle_command_ack,
					  vehicle_command_ack_s::ORB_QUEUE_LENGTH);

	} else {
		orb_publish(ORB_ID(vehicle_command_ack), vehicle_command_ack_pub, &vehicle_command_ack);
	}

}

}
}