feat(logger): add support for small flash storage

Move log cleanup from boot to log start, delete individual .ulg files
(oldest first) instead of entire directories, and add a max log file
size. This makes it practical to run the logger on small flash targets
(e.g. 128 MB W25N NAND) while still allowing logs to be downloaded via
MAVLink FTP before they are removed.

- Move cleanup from boot to log start so logs can be downloaded first.
- Delete individual .ulg files (oldest first) instead of whole dirs.
- Add SDLOG_MAX_SIZE parameter (default 4095 MiB, just under FAT32 limit).
- Prioritize directories from the naming scheme not currently in use
  (e.g. delete sess dirs first when using date dirs).
- Simplify get_log_time to rely on clock_gettime (GPS driver sets it).
- Safer string handling in directory parsing.
- Log elapsed cleanup time for diagnostics.
- Split pure parsing helpers into util_parse.{h,cpp} and add unit tests
  (loggerUtilTest.cpp). Move px4_add_unit_gtest outside the BUILD_TESTING
  guard for consistency with the rest of the tree; the macro already
  checks BUILD_TESTING internally.

Sponsored by CubePilot.
This commit is contained in:
Julian Oes 2026-04-11 05:58:49 +12:00
parent 0e31dd560d
commit 5762d72004
No known key found for this signature in database
GPG Key ID: F0ED380FEA56DE41
12 changed files with 805 additions and 157 deletions

View File

@ -41,3 +41,4 @@ param set-default COM_ARM_SDCARD 0
# W25N NAND flash with littlefs (128 MB): larger buffer, auto-rotate
set LOGGER_BUF 32
param set-default SDLOG_DIRS_MAX 3
param set-default SDLOG_MAX_SIZE 30

View File

@ -59,11 +59,6 @@
# define BOARD_HAS_NBAT_V 1
# define BOARD_HAS_NBAT_I 1
/* Enable small flash logging support (for W25N NAND flash) */
#ifdef CONFIG_MTD_W25N
# define BOARD_SMALL_FLASH_LOGGING 1
#endif
/* Holybro KakuteH7 GPIOs ************************************************************************/
/* LEDs are driven with push open drain to support Anode to 5V or 3.3V */

View File

@ -83,6 +83,27 @@ This configuration will log sensor_accel 0 at full rate, sensor_accel 1 at 10Hz,
There are several scripts to analyze and convert logging files in the [pyulog](https://github.com/PX4/pyulog) repository.
## Log Cleanup
PX4 automatically manages log storage by cleaning up old logs when starting to log.
Cleanup is triggered based on two criteria:
- **Storage-based cleanup**: Ensures minimum free space (300 MB or 10% of disk, whichever is smaller) is available.
- **Count-based cleanup**: If [SDLOG_DIRS_MAX](../advanced_config/parameter_reference.md#SDLOG_DIRS_MAX) is set, limits the total number of log directories.
The cleanup algorithm prioritizes deleting logs from the directory naming scheme not currently in use.
PX4 uses two directory naming schemes:
- **Session directories** (`sess001`, `sess002`, etc.) - used when the system doesn't have valid time information
- **Date directories** (`2024-01-15`, `2024-01-16`, etc.) - used when the system has valid time (e.g., from GPS)
When cleanup is needed:
- If the system has valid time (using date directories): old session directories are deleted first
- If the system doesn't have valid time (using session directories): old date directories are deleted first
This ensures that stale logs from a different time mode are cleaned up before current logs.
## File size limitations
The maximum file size depends on the file system and OS.

View File

@ -55,6 +55,7 @@ px4_add_module(
log_writer_file.cpp
log_writer_mavlink.cpp
util.cpp
util_parse.cpp
watchdog.cpp
DEPENDS
version
@ -62,3 +63,11 @@ px4_add_module(
)
px4_add_unit_gtest(SRC ULogMessagesTest.cpp)
if(BUILD_TESTING)
# Separate library for pure parsing functions (testable without PX4 dependencies)
add_library(logger_util_parse STATIC util_parse.cpp)
target_include_directories(logger_util_parse PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
endif()
px4_add_unit_gtest(SRC loggerUtilTest.cpp LINKLIBS logger_util_parse)

View File

@ -38,6 +38,7 @@
#include "messages.h"
#include <dirent.h>
#include <inttypes.h>
#include <sys/stat.h>
#include <errno.h>
#include <string.h>
@ -597,9 +598,14 @@ void Logger::run()
}
}
if (util::check_free_space(LOG_ROOT[(int)LogType::Full], _param_sdlog_dirs_max.get(), _mavlink_log_pub,
_file_name[(int)LogType::Full].sess_dir_index) == 1) {
return;
// Get the next session directory index
util::LogDirInfo dir_info;
if (util::scan_log_directories(LOG_ROOT[(int)LogType::Full], dir_info)) {
_file_name[(int)LogType::Full].sess_dir_index = dir_info.sess_idx_max + 1;
} else {
_file_name[(int)LogType::Full].sess_dir_index = 0;
}
}
@ -814,7 +820,7 @@ void Logger::run()
if (_log_message_sub.update(&log_message)) {
const char *message = (const char *)log_message.text;
int message_len = strlen(message);
int message_len = strnlen(message, sizeof(log_message.text));
if (message_len > 0) {
uint16_t write_msg_size = sizeof(ulog_message_logging_s) - sizeof(ulog_message_logging_s::message)
@ -879,6 +885,14 @@ void Logger::run()
debug_print_buffer(total_bytes, timer_start);
// Rotate log file when it exceeds max size (if SDLOG_MAX_SIZE > 0)
if (_max_log_file_size > 0 &&
_writer.get_total_written_file(LogType::Full) > _max_log_file_size) {
PX4_INFO("Log file size limit reached, rotating");
stop_log_file(LogType::Full);
start_log_file(LogType::Full);
}
was_started = true;
} else { // not logging
@ -1250,7 +1264,8 @@ int Logger::create_log_dir(LogType type, tm *tt, char *log_dir, int log_dir_len)
if (tt) {
strftime(file_name.log_dir, sizeof(LogFileName::log_dir), "%Y-%m-%d", tt);
strncpy(log_dir + n, file_name.log_dir, log_dir_len - n);
strncpy(log_dir + n, file_name.log_dir, log_dir_len - n - 1);
log_dir[log_dir_len - 1] = '\0';
int mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO);
if (mkdir_ret != OK && errno != EEXIST) {
@ -1262,7 +1277,8 @@ int Logger::create_log_dir(LogType type, tm *tt, char *log_dir, int log_dir_len)
uint16_t dir_number = file_name.sess_dir_index;
if (file_name.has_log_dir) {
strncpy(log_dir + n, file_name.log_dir, log_dir_len - n);
strncpy(log_dir + n, file_name.log_dir, log_dir_len - n - 1);
log_dir[log_dir_len - 1] = '\0';
}
/* look for the next dir that does not exist */
@ -1275,7 +1291,8 @@ int Logger::create_log_dir(LogType type, tm *tt, char *log_dir, int log_dir_len)
return -1;
}
strncpy(log_dir + n, file_name.log_dir, log_dir_len - n);
strncpy(log_dir + n, file_name.log_dir, log_dir_len - n - 1);
log_dir[log_dir_len - 1] = '\0';
int mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO);
if (mkdir_ret == 0) {
@ -1359,20 +1376,27 @@ int Logger::get_log_file_name(LogType type, char *file_name, size_t file_name_si
return -1;
}
// Find the highest existing log file number and use next
uint16_t file_number = 100; // start with file log100
uint16_t max_existing = 99;
/* look for the next file that does not exist */
while (file_number <= MAX_NO_LOGFILE) {
/* format log file path: e.g. /fs/microsd/log/sess001/log001.ulg */
snprintf(log_file_name, sizeof(LogFileName::log_file_name), "log%03" PRIu16 "%s.ulg%s", file_number, replay_suffix,
crypto_suffix);
snprintf(file_name + n, file_name_size - n, "/%s", log_file_name);
DIR *dp = opendir(file_name);
if (!util::file_exist(file_name)) {
break;
if (dp != nullptr) {
struct dirent *entry;
while ((entry = readdir(dp)) != nullptr) {
uint16_t num;
if (sscanf(entry->d_name, "log%hu", &num) == 1) {
if (num > max_existing) {
max_existing = num;
}
}
}
file_number++;
closedir(dp);
file_number = max_existing + 1;
}
if (file_number > MAX_NO_LOGFILE) {
@ -1380,6 +1404,11 @@ int Logger::get_log_file_name(LogType type, char *file_name, size_t file_name_si
return -1;
}
/* format log file path: e.g. /fs/microsd/log/sess001/log001.ulg */
snprintf(log_file_name, sizeof(LogFileName::log_file_name), "log%03" PRIu16 "%s.ulg%s", file_number, replay_suffix,
crypto_suffix);
snprintf(file_name + n, file_name_size - n, "/%s", log_file_name);
if (notify) {
mavlink_log_info(&_mavlink_log_pub, "[logger] %s\t", file_name);
uint16_t sess = 0;
@ -1410,6 +1439,26 @@ void Logger::start_log_file(LogType type)
}
if (type == LogType::Full) {
int32_t max_size_mb = _param_sdlog_max_size.get();
if (max_size_mb > 0) {
_max_log_file_size = (size_t)max_size_mb * 1024ULL * 1024ULL;
PX4_INFO("Max log file size: %" PRId32 " MB", max_size_mb);
} else {
_max_log_file_size = 0; // unlimited
}
// Cleanup old logs if needed (storage-based and/or count-based)
hrt_abstime cleanup_start = hrt_absolute_time();
if (util::cleanup_old_logs(LOG_ROOT[(int)LogType::Full], _mavlink_log_pub,
(uint32_t)max_size_mb, _param_sdlog_dirs_max.get()) == 1) {
return; // Not enough space even after cleanup
}
PX4_INFO("Log cleanup took %" PRIu64 " ms", hrt_elapsed_time(&cleanup_start) / 1000);
// initialize cpu load as early as possible to get more data
initialize_load_output(PrintLoadReason::Preflight);
}

View File

@ -387,6 +387,8 @@ private:
hrt_abstime _logger_status_last {0};
int _lockstep_component{-1};
size_t _max_log_file_size {0}; ///< max log file size in bytes (0 = unlimited)
uint32_t _message_gaps{0};
timer_callback_data_s _timer_callback_data{};
@ -400,6 +402,7 @@ private:
DEFINE_PARAMETERS(
(ParamInt<px4::params::SDLOG_UTC_OFFSET>) _param_sdlog_utc_offset,
(ParamInt<px4::params::SDLOG_DIRS_MAX>) _param_sdlog_dirs_max,
(ParamInt<px4::params::SDLOG_MAX_SIZE>) _param_sdlog_max_size,
(ParamInt<px4::params::SDLOG_PROFILE>) _param_sdlog_profile,
(ParamInt<px4::params::SDLOG_MISSION>) _param_sdlog_mission,
(ParamBool<px4::params::SDLOG_BOOT_BAT>) _param_sdlog_boot_bat,

View File

@ -0,0 +1,283 @@
/****************************************************************************
*
* Copyright (c) 2025 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.
*
****************************************************************************/
/**
* Test code for the logger utility parsing functions
* Run this test using: build/px4_sitl_test/unit-util
*/
#include <gtest/gtest.h>
#include "util_parse.h"
using namespace px4::logger::util;
// Session directory parsing tests
TEST(LoggerUtilTest, ParseSessDirValid)
{
int idx;
EXPECT_TRUE(parse_sess_dir_name("sess000", idx));
EXPECT_EQ(idx, 0);
EXPECT_TRUE(parse_sess_dir_name("sess001", idx));
EXPECT_EQ(idx, 1);
EXPECT_TRUE(parse_sess_dir_name("sess123", idx));
EXPECT_EQ(idx, 123);
EXPECT_TRUE(parse_sess_dir_name("sess999", idx));
EXPECT_EQ(idx, 999);
}
TEST(LoggerUtilTest, ParseSessDirInvalid)
{
int idx;
EXPECT_FALSE(parse_sess_dir_name("session001", idx));
EXPECT_FALSE(parse_sess_dir_name("2024-01-15", idx));
EXPECT_FALSE(parse_sess_dir_name(".", idx));
EXPECT_FALSE(parse_sess_dir_name("..", idx));
EXPECT_FALSE(parse_sess_dir_name("log001", idx));
EXPECT_FALSE(parse_sess_dir_name("", idx));
}
// Date directory parsing tests
TEST(LoggerUtilTest, ParseDateDirValid)
{
int y, m, d;
EXPECT_TRUE(parse_date_dir_name("2024-01-15", y, m, d));
EXPECT_EQ(y, 2024);
EXPECT_EQ(m, 1);
EXPECT_EQ(d, 15);
EXPECT_TRUE(parse_date_dir_name("2023-12-31", y, m, d));
EXPECT_EQ(y, 2023);
EXPECT_EQ(m, 12);
EXPECT_EQ(d, 31);
EXPECT_TRUE(parse_date_dir_name("2025-06-01", y, m, d));
EXPECT_EQ(y, 2025);
EXPECT_EQ(m, 6);
EXPECT_EQ(d, 1);
}
TEST(LoggerUtilTest, ParseDateDirInvalid)
{
int y, m, d;
EXPECT_FALSE(parse_date_dir_name("sess001", y, m, d));
EXPECT_FALSE(parse_date_dir_name("2024-01", y, m, d));
EXPECT_FALSE(parse_date_dir_name("2024", y, m, d));
EXPECT_FALSE(parse_date_dir_name(".", y, m, d));
EXPECT_FALSE(parse_date_dir_name("..", y, m, d));
EXPECT_FALSE(parse_date_dir_name("", y, m, d));
}
// Date comparison tests
TEST(LoggerUtilTest, IsDateOlderYearDifference)
{
// Earlier year is older
EXPECT_TRUE(is_date_older(2023, 12, 31, 2024, 1, 1));
EXPECT_FALSE(is_date_older(2024, 1, 1, 2023, 12, 31));
}
TEST(LoggerUtilTest, IsDateOlderMonthDifference)
{
// Same year, earlier month is older
EXPECT_TRUE(is_date_older(2024, 1, 15, 2024, 2, 1));
EXPECT_FALSE(is_date_older(2024, 2, 1, 2024, 1, 15));
}
TEST(LoggerUtilTest, IsDateOlderDayDifference)
{
// Same year and month, earlier day is older
EXPECT_TRUE(is_date_older(2024, 1, 1, 2024, 1, 15));
EXPECT_FALSE(is_date_older(2024, 1, 15, 2024, 1, 1));
}
TEST(LoggerUtilTest, IsDateOlderSameDate)
{
// Same date is not older
EXPECT_FALSE(is_date_older(2024, 1, 15, 2024, 1, 15));
}
TEST(LoggerUtilTest, IsDateOlderEdgeCases)
{
// Year boundary
EXPECT_TRUE(is_date_older(2023, 12, 31, 2024, 1, 1));
// Month boundary
EXPECT_TRUE(is_date_older(2024, 1, 31, 2024, 2, 1));
// Large year difference
EXPECT_TRUE(is_date_older(2000, 6, 15, 2024, 6, 15));
}
// process_dir_entry tests - combined sess vs date logic
TEST(LoggerUtilTest, ProcessDirEntrySessionOnly)
{
LogDirInfo info{};
process_dir_entry("sess000", info);
EXPECT_EQ(info.num_sess, 1);
EXPECT_EQ(info.sess_idx_min, 0);
EXPECT_EQ(info.sess_idx_max, 0);
EXPECT_EQ(info.num_dates, 0);
process_dir_entry("sess005", info);
EXPECT_EQ(info.num_sess, 2);
EXPECT_EQ(info.sess_idx_min, 0);
EXPECT_EQ(info.sess_idx_max, 5);
process_dir_entry("sess003", info);
EXPECT_EQ(info.num_sess, 3);
EXPECT_EQ(info.sess_idx_min, 0);
EXPECT_EQ(info.sess_idx_max, 5);
}
TEST(LoggerUtilTest, ProcessDirEntryDateOnly)
{
LogDirInfo info{};
process_dir_entry("2024-06-15", info);
EXPECT_EQ(info.num_dates, 1);
EXPECT_EQ(info.oldest_year, 2024);
EXPECT_EQ(info.oldest_month, 6);
EXPECT_EQ(info.oldest_day, 15);
EXPECT_EQ(info.num_sess, 0);
// Add older date
process_dir_entry("2024-01-10", info);
EXPECT_EQ(info.num_dates, 2);
EXPECT_EQ(info.oldest_year, 2024);
EXPECT_EQ(info.oldest_month, 1);
EXPECT_EQ(info.oldest_day, 10);
// Add newer date (oldest should not change)
process_dir_entry("2024-12-25", info);
EXPECT_EQ(info.num_dates, 3);
EXPECT_EQ(info.oldest_year, 2024);
EXPECT_EQ(info.oldest_month, 1);
EXPECT_EQ(info.oldest_day, 10);
}
TEST(LoggerUtilTest, ProcessDirEntryMixedSessAndDate)
{
LogDirInfo info{};
process_dir_entry("sess001", info);
process_dir_entry("2024-03-15", info);
process_dir_entry("sess005", info);
process_dir_entry("2023-12-01", info);
EXPECT_EQ(info.num_sess, 2);
EXPECT_EQ(info.sess_idx_min, 1);
EXPECT_EQ(info.sess_idx_max, 5);
EXPECT_EQ(info.num_dates, 2);
EXPECT_EQ(info.oldest_year, 2023);
EXPECT_EQ(info.oldest_month, 12);
EXPECT_EQ(info.oldest_day, 1);
}
TEST(LoggerUtilTest, ProcessDirEntryIgnoresInvalid)
{
LogDirInfo info{};
// These should be ignored
process_dir_entry(".", info);
process_dir_entry("..", info);
process_dir_entry("log001", info);
process_dir_entry("session001", info);
process_dir_entry("2024-01", info);
process_dir_entry("random_dir", info);
EXPECT_EQ(info.num_sess, 0);
EXPECT_EQ(info.num_dates, 0);
EXPECT_EQ(info.sess_idx_max, -1); // unchanged from default
EXPECT_EQ(info.sess_idx_min, INT_MAX); // unchanged from default
}
TEST(LoggerUtilTest, ProcessDirEntrySessMinMaxTracking)
{
LogDirInfo info{};
// Add in non-sequential order
process_dir_entry("sess050", info);
EXPECT_EQ(info.sess_idx_min, 50);
EXPECT_EQ(info.sess_idx_max, 50);
process_dir_entry("sess010", info);
EXPECT_EQ(info.sess_idx_min, 10);
EXPECT_EQ(info.sess_idx_max, 50);
process_dir_entry("sess100", info);
EXPECT_EQ(info.sess_idx_min, 10);
EXPECT_EQ(info.sess_idx_max, 100);
process_dir_entry("sess025", info);
EXPECT_EQ(info.sess_idx_min, 10);
EXPECT_EQ(info.sess_idx_max, 100);
}
TEST(LoggerUtilTest, ProcessDirEntryDateOldestTracking)
{
LogDirInfo info{};
// Start with a date in the middle
process_dir_entry("2024-06-15", info);
EXPECT_EQ(info.oldest_year, 2024);
EXPECT_EQ(info.oldest_month, 6);
EXPECT_EQ(info.oldest_day, 15);
// Add older year
process_dir_entry("2023-12-31", info);
EXPECT_EQ(info.oldest_year, 2023);
EXPECT_EQ(info.oldest_month, 12);
EXPECT_EQ(info.oldest_day, 31);
// Add same year, older month
process_dir_entry("2023-01-15", info);
EXPECT_EQ(info.oldest_year, 2023);
EXPECT_EQ(info.oldest_month, 1);
EXPECT_EQ(info.oldest_day, 15);
// Add same year/month, older day
process_dir_entry("2023-01-01", info);
EXPECT_EQ(info.oldest_year, 2023);
EXPECT_EQ(info.oldest_month, 1);
EXPECT_EQ(info.oldest_day, 1);
// Add newer date (oldest should not change)
process_dir_entry("2025-01-01", info);
EXPECT_EQ(info.oldest_year, 2023);
EXPECT_EQ(info.oldest_month, 1);
EXPECT_EQ(info.oldest_day, 1);
}

View File

@ -106,16 +106,30 @@ parameters:
description:
short: Maximum number of log directories to keep
long: 'If there are more log directories than this value, the system will
delete the oldest directories during startup. In addition, the system will
delete old logs if there is not enough free space left. The minimum amount
is 300 MB. If this is set to 0, old directories will only be removed if
the free space falls below the minimum. Note: this does not apply to mission
log files.'
delete the oldest directories when starting to log. Cleanup prioritizes
directories from the naming scheme not currently in use (e.g., sess dirs
are deleted first when date dirs are being used). If set to 0, directories
are only removed based on storage space. The minimum free space threshold
is 300 MB or 10% of disk size, whichever is smaller. Note: this does not
apply to mission log files.'
type: int32
default: 0
min: 0
max: 1000
reboot_required: true
reboot_required: false
SDLOG_MAX_SIZE:
description:
short: Maximum log file size
long: 'Maximum size of a single log file in megabytes. When reached,
the log file is closed and a new one is started.
Cleanup of old logs always happens at log start (not boot) to allow
downloading logs via FTP before deletion.
Must stay below the FAT32 file size limit of 4 GiB.'
type: int32
default: 4095
min: 0
max: 4095
reboot_required: false
SDLOG_UUID:
description:
short: Log UUID

View File

@ -34,14 +34,12 @@
#include "util.h"
#include <dirent.h>
#include <inttypes.h>
#include <sys/stat.h>
#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/sensor_gps.h>
#include <drivers/drv_hrt.h>
#include <px4_platform_common/events.h>
#include <px4_platform_common/log.h>
@ -57,8 +55,6 @@
#define GPS_EPOCH_SECS ((time_t)1234567890ULL)
typedef decltype(statfs::f_bavail) px4_statfs_buf_f_bavail_t;
namespace px4
{
namespace logger
@ -72,32 +68,33 @@ bool file_exist(const char *filename)
return stat(filename, &buffer) == 0;
}
bool get_log_time(uint64_t &utc_time_usec, int utc_offset_sec, bool boot_time)
bool get_free_space(const char *path, uint64_t *avail_bytes, uint64_t *total_bytes)
{
uORB::Subscription vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
struct statfs statfs_buf;
bool use_clock_time = true;
/* Get the latest GPS publication */
sensor_gps_s gps_pos;
if (vehicle_gps_position_sub.copy(&gps_pos)) {
utc_time_usec = gps_pos.time_utc_usec;
if (gps_pos.fix_type >= 2 && utc_time_usec >= (uint64_t) GPS_EPOCH_SECS * 1000000ULL) {
use_clock_time = false;
}
if (statfs(path, &statfs_buf) != 0) {
return false;
}
if (use_clock_time) {
/* take clock time if there's no fix (yet) */
struct timespec ts = {};
px4_clock_gettime(CLOCK_REALTIME, &ts);
utc_time_usec = ts.tv_sec * 1000000ULL + ts.tv_nsec / 1000ULL;
if (avail_bytes != nullptr) {
*avail_bytes = (uint64_t)statfs_buf.f_bavail * statfs_buf.f_bsize;
}
if (utc_time_usec < (uint64_t) GPS_EPOCH_SECS * 1000000ULL) {
return false;
}
if (total_bytes != nullptr) {
*total_bytes = (uint64_t)statfs_buf.f_blocks * statfs_buf.f_bsize;
}
return true;
}
bool get_log_time(uint64_t &utc_time_usec, int utc_offset_sec, bool boot_time)
{
struct timespec ts = {};
px4_clock_gettime(CLOCK_REALTIME, &ts);
utc_time_usec = ts.tv_sec * 1000000ULL + ts.tv_nsec / 1000ULL;
if (utc_time_usec < (uint64_t) GPS_EPOCH_SECS * 1000000ULL) {
return false;
}
/* strip the time elapsed since boot */
@ -119,130 +116,204 @@ bool get_log_time(struct tm *tt, int utc_offset_sec, bool boot_time)
return result && gmtime_r(&utc_time_sec, tt) != nullptr;
}
int check_free_space(const char *log_root_dir, int32_t max_log_dirs_to_keep, orb_advert_t &mavlink_log_pub,
int &sess_dir_index)
bool scan_log_directories(const char *log_root_dir, LogDirInfo &info)
{
struct statfs statfs_buf;
DIR *dp = opendir(log_root_dir);
if (max_log_dirs_to_keep == 0) {
max_log_dirs_to_keep = INT32_MAX;
if (dp == nullptr) {
return false;
}
// remove old logs if the free space falls below a threshold
do {
if (statfs(log_root_dir, &statfs_buf) != 0) {
return PX4_ERROR;
// Reset info to defaults
info = LogDirInfo{};
struct dirent *result = nullptr;
while ((result = readdir(dp))) {
process_dir_entry(result->d_name, info);
}
closedir(dp);
return true;
}
int cleanup_old_logs(const char *log_root_dir, orb_advert_t &mavlink_log_pub,
uint32_t target_free_mb, int32_t max_log_dirs_to_keep)
{
uint64_t avail_bytes = 0;
uint64_t total_bytes = 0;
if (!get_free_space(log_root_dir, &avail_bytes, &total_bytes)) {
return PX4_ERROR;
}
// Calculate cleanup threshold
uint64_t cleanup_threshold;
if (target_free_mb > 0) {
cleanup_threshold = (uint64_t)target_free_mb * 1024ULL * 1024ULL;
} else {
// Default: 300 MiB or 10% of disk, whichever is smaller
cleanup_threshold = 300ULL * 1024ULL * 1024ULL;
if (total_bytes / 10 < cleanup_threshold) {
cleanup_threshold = total_bytes / 10;
}
}
// Early out if we have enough space and no directory limit
bool need_space_cleanup = avail_bytes < cleanup_threshold;
if (!need_space_cleanup && max_log_dirs_to_keep <= 0) {
return PX4_OK;
}
// Scan directories for cleanup
LogDirInfo info;
if (!scan_log_directories(log_root_dir, info)) {
return PX4_OK; // ignore if we cannot access the log directory
}
int total_dirs = info.num_sess + info.num_dates;
bool need_count_cleanup = (max_log_dirs_to_keep > 0) && (total_dirs > max_log_dirs_to_keep);
if (!need_space_cleanup && !need_count_cleanup) {
return PX4_OK;
}
PX4_INFO("Log cleanup: %u MiB free, threshold %u MiB, %d dirs (max %" PRId32 ")",
(unsigned)(avail_bytes / 1024U / 1024U), (unsigned)(cleanup_threshold / 1024U / 1024U),
total_dirs, max_log_dirs_to_keep > 0 ? max_log_dirs_to_keep : -1);
// Determine if we currently have valid time (using date dirs) or not (using sess dirs)
// Delete from the "other" scheme first to avoid deleting current log
uint64_t utc_time_usec;
bool have_time = get_log_time(utc_time_usec, 0, false);
// Cleanup oldest .ulg files one by one until conditions are met
int empty_dir_failures = 0;
while (need_space_cleanup || need_count_cleanup) {
char oldest_file[LOG_DIR_LEN] = "";
char oldest_dir[LOG_DIR_LEN];
if (!scan_log_directories(log_root_dir, info)) {
break;
}
DIR *dp = opendir(log_root_dir);
total_dirs = info.num_sess + info.num_dates;
bool found_sess = info.num_sess > 0;
bool found_date = info.num_dates > 0;
if (!found_sess && !found_date) {
PX4_WARN("No log directories found to clean up");
break; // no log directories found
}
// Delete from the "other" naming scheme first (it's old/stale)
// - Have time (using date dirs): delete sess dirs first
// - No time (using sess dirs): delete date dirs first, then sess dirs
if (have_time && found_sess) {
// Using date dirs, delete old sess dirs first
snprintf(oldest_dir, sizeof(oldest_dir), "%s/sess%03u", log_root_dir, info.sess_idx_min);
} else if (!have_time && found_date) {
// Using sess dirs, delete old date dirs first
snprintf(oldest_dir, sizeof(oldest_dir), "%s/%04u-%02u-%02u", log_root_dir,
info.oldest_year, info.oldest_month, info.oldest_day);
} else if (found_sess) {
// Delete from oldest sess dir (including current - old files are ok to delete)
snprintf(oldest_dir, sizeof(oldest_dir), "%s/sess%03u", log_root_dir, info.sess_idx_min);
} else if (found_date) {
// Delete from oldest date dir
snprintf(oldest_dir, sizeof(oldest_dir), "%s/%04u-%02u-%02u", log_root_dir,
info.oldest_year, info.oldest_month, info.oldest_day);
} else {
// Nothing left to delete
break;
}
PX4_DEBUG("Checking directory %s for old logs", oldest_dir);
// Find oldest .ulg file in that directory
DIR *dp = opendir(oldest_dir);
if (dp == nullptr) {
break; // ignore if we cannot access the log directory
PX4_WARN("Cannot open directory %s", oldest_dir);
break;
}
// Max log filename: "12_09_00_replayed.ulg" (21 chars + null = 22 bytes)
// Using 64 for margin against unexpected filenames on disk.
static constexpr unsigned MAX_LOG_FILENAME_LEN = 64;
char oldest_ulg[MAX_LOG_FILENAME_LEN] = "";
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 = 99;
while ((result = readdir(dp))) {
int year, month, day, sess_idx;
size_t len = strlen(result->d_name);
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;
}
}
if (len > 4 && strcmp(result->d_name + len - 4, ".ulg") == 0) {
if (oldest_ulg[0] == '\0' || strcmp(result->d_name, oldest_ulg) < 0) {
strncpy(oldest_ulg, result->d_name, sizeof(oldest_ulg) - 1);
oldest_ulg[sizeof(oldest_ulg) - 1] = '\0';
}
}
}
closedir(dp);
sess_dir_index = sess_idx_max + 1;
if (oldest_ulg[0] == '\0') {
// No .ulg files, try to remove directory
if (remove_directory(oldest_dir) == 0) {
PX4_INFO("removed directory %s (no .ulg files)", oldest_dir);
empty_dir_failures = 0;
} else {
// Removal failed (littlefs may report "not empty" for empty dirs)
// Toggle have_time to try the other naming scheme next iteration
empty_dir_failures++;
uint64_t min_free_bytes = 300ULL * 1024ULL * 1024ULL;
uint64_t total_bytes = (uint64_t)statfs_buf.f_blocks * statfs_buf.f_bsize;
if (empty_dir_failures >= 3) {
PX4_WARN("Cannot remove empty directories, giving up");
break;
}
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;
have_time = !have_time;
PX4_DEBUG("Cannot remove %s, trying other scheme", oldest_dir);
}
continue;
}
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
}
// Build full path and delete the file
snprintf(oldest_file, sizeof(oldest_file), "%s/%s", oldest_dir, oldest_ulg);
PX4_INFO("removing old log %s/%s", oldest_dir, oldest_ulg);
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_dir, sess_idx_min);
} else {
n = snprintf(directory_to_delete, sizeof(directory_to_delete), "%s/%04u-%02u-%02u", log_root_dir, year_min, month_min,
day_min);
}
if (n >= (int)sizeof(directory_to_delete)) {
PX4_ERR("log path too long (%i)", n);
if (unlink(oldest_file) != 0) {
PX4_ERR("Failed to delete %s", oldest_file);
break;
}
PX4_INFO("removing log directory %s to get more space (left=%u MiB)", directory_to_delete,
(unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize / 1024U / 1024U));
if (remove_directory(directory_to_delete)) {
PX4_ERR("Failed to delete directory");
// Re-check conditions
if (!get_free_space(log_root_dir, &avail_bytes, nullptr)) {
break;
}
} while (true);
need_space_cleanup = avail_bytes < cleanup_threshold;
need_count_cleanup = (max_log_dirs_to_keep > 0) && (total_dirs > max_log_dirs_to_keep);
}
/* 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\t",
(unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize / 1024U / 1024U));
/* EVENT
* @description Either manually free up some space, or enable automatic log rotation
* via <param>SDLOG_DIRS_MAX</param>.
*/
// Final check: if still not enough space, refuse to log
if (avail_bytes < 10ULL * 1024ULL * 1024ULL) { // Less than 10 MiB is critical
mavlink_log_critical(&mavlink_log_pub, "[logger] Storage full: %u MiB free\t",
(unsigned)(avail_bytes / 1024U / 1024U));
events::send<uint32_t>(events::ID("logger_storage_full"), events::Log::Error,
"Not logging, storage is almost full: {1} MiB", (uint32_t)(statfs_buf.f_bavail * statfs_buf.f_bsize / 1024U / 1024U));
"Storage full: {1} MiB free", (uint32_t)(avail_bytes / 1024U / 1024U));
return 1;
}

View File

@ -34,6 +34,7 @@
#pragma once
#include <stdint.h>
#include <climits>
#include <time.h>
#include <uORB/uORB.h>
@ -44,6 +45,9 @@
#define LOG_DIR_LEN 256
#endif
// Include parsing utilities (separate file for testability)
#include "util_parse.h"
namespace px4
{
namespace logger
@ -63,25 +67,41 @@ int remove_directory(const char *dir);
bool file_exist(const char *filename);
/**
* Check if there is enough free space left on the SD Card.
* It will remove old log files if there is not enough space,
* and if that fails return 1, and send a user message
* @param log_root_dir log root directory: it's expected to contain directories in the form of sess%i or %d-%d-%d (year, month, day)
* @param max_log_dirs_to_keep maximum log directories to keep (set to 0 for unlimited)
* @param mavlink_log_pub
* @param sess_dir_index output argument: will be set to the next free directory sess%i index.
* @return 0 on success, 1 if not enough space, <0 on error
* Get available and total storage space for a path.
* @param path path to check
* @param avail_bytes available bytes (output), can be nullptr if not needed
* @param total_bytes total bytes (output), can be nullptr if not needed
* @return true on success, false on error
*/
int check_free_space(const char *log_root_dir, int32_t max_log_dirs_to_keep, orb_advert_t &mavlink_log_pub,
int &sess_dir_index);
bool get_free_space(const char *path, uint64_t *avail_bytes, uint64_t *total_bytes);
/**
* Utility for fetching UTC time in microseconds from sensor_gps or CLOCK_REALTIME
* Scan log directory and gather information about subdirectories.
* @param log_root_dir log root directory to scan
* @param info output: populated with directory information
* @return true on success, false if directory cannot be opened
*/
bool scan_log_directories(const char *log_root_dir, LogDirInfo &info);
/**
* Cleanup old logs to ensure sufficient free space. Deletes oldest files,
* preferring the opposite directory type first (sess dirs when time is known,
* date dirs when it is not), then falls back to its own type.
* @param log_root_dir log root directory
* @param mavlink_log_pub mavlink log publisher
* @param target_free_mb target free space in MB (0 = use default minimum)
* @param max_log_dirs_to_keep maximum log directories to keep (0 = unlimited)
* @return 0 on success, 1 if not enough space even after cleanup
*/
int cleanup_old_logs(const char *log_root_dir, orb_advert_t &mavlink_log_pub,
uint32_t target_free_mb, int32_t max_log_dirs_to_keep);
/**
* Get UTC time in microseconds from CLOCK_REALTIME
* @param utc_time_usec returned microseconds
* @param utc_offset_sec UTC time offset [s]
* @param boot_time use time when booted instead of current time
* @return true on success, false otherwise (eg. if no gps)
* @return true on success, false if system time is not set
*/
bool get_log_time(uint64_t &utc_time_usec, int utc_offset_sec, bool boot_time);
@ -90,7 +110,7 @@ bool get_log_time(uint64_t &utc_time_usec, int utc_offset_sec, bool boot_time);
* @param tt returned time
* @param utc_offset_sec UTC time offset [s]
* @param boot_time use time when booted instead of current time
* @return true on success, false otherwise (eg. if no gps)
* @return true on success, false if system time is not set
*/
bool get_log_time(struct tm *tt, int utc_offset_sec = 0, bool boot_time = false);

View File

@ -0,0 +1,90 @@
/****************************************************************************
*
* Copyright (c) 2025 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 "util_parse.h"
#include <cstdio>
namespace px4
{
namespace logger
{
namespace util
{
bool parse_sess_dir_name(const char *name, int &sess_idx)
{
return sscanf(name, "sess%d", &sess_idx) == 1;
}
bool parse_date_dir_name(const char *name, int &year, int &month, int &day)
{
return sscanf(name, "%d-%d-%d", &year, &month, &day) == 3;
}
bool is_date_older(int y1, int m1, int d1, int y2, int m2, int d2)
{
return y1 < y2 ||
(y1 == y2 && m1 < m2) ||
(y1 == y2 && m1 == m2 && d1 < d2);
}
void process_dir_entry(const char *name, LogDirInfo &info)
{
int sess_idx;
int year, month, day;
if (parse_sess_dir_name(name, sess_idx)) {
info.num_sess++;
if (sess_idx > info.sess_idx_max) {
info.sess_idx_max = sess_idx;
}
if (sess_idx < info.sess_idx_min) {
info.sess_idx_min = sess_idx;
}
} else if (parse_date_dir_name(name, year, month, day)) {
info.num_dates++;
if (is_date_older(year, month, day, info.oldest_year, info.oldest_month, info.oldest_day)) {
info.oldest_year = year;
info.oldest_month = month;
info.oldest_day = day;
}
}
}
} //namespace util
} //namespace logger
} //namespace px4

View File

@ -0,0 +1,92 @@
/****************************************************************************
*
* Copyright (c) 2025 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.
*
****************************************************************************/
#pragma once
#include <climits>
namespace px4
{
namespace logger
{
namespace util
{
/**
* Information about log directories gathered by scan_log_directories()
*/
struct LogDirInfo {
int sess_idx_max{-1}; ///< highest sess index found (-1 if none)
int sess_idx_min{INT_MAX}; ///< lowest sess index found
int num_sess{0}; ///< count of sess directories
int oldest_year{INT_MAX}; ///< oldest date directory year
int oldest_month{INT_MAX}; ///< oldest date directory month
int oldest_day{INT_MAX}; ///< oldest date directory day
int num_dates{0}; ///< count of date directories
};
/**
* Process a single directory entry and update LogDirInfo accordingly.
* Tries to parse as session dir first, then as date dir.
* @param name directory entry name to process
* @param info LogDirInfo struct to update
*/
void process_dir_entry(const char *name, LogDirInfo &info);
/**
* Parse a session directory name (e.g., "sess001", "sess123")
* @param name directory name to parse
* @param sess_idx output: session index if parsing succeeds
* @return true if name matches "sess%d" pattern
*/
bool parse_sess_dir_name(const char *name, int &sess_idx);
/**
* Parse a date directory name (e.g., "2024-01-15")
* @param name directory name to parse
* @param year output: year if parsing succeeds
* @param month output: month if parsing succeeds
* @param day output: day if parsing succeeds
* @return true if name matches "%d-%d-%d" pattern
*/
bool parse_date_dir_name(const char *name, int &year, int &month, int &day);
/**
* Compare two dates
* @return true if (y1,m1,d1) < (y2,m2,d2)
*/
bool is_date_older(int y1, int m1, int d1, int y2, int m2, int d2);
} //namespace util
} //namespace logger
} //namespace px4