diff --git a/src/ParseInfo/SensorScheme.cpp b/src/ParseInfo/SensorScheme.cpp index 292dc403..405fdd3b 100644 --- a/src/ParseInfo/SensorScheme.cpp +++ b/src/ParseInfo/SensorScheme.cpp @@ -411,7 +411,27 @@ ssize_t serializeParseInfoStorage(char* buffer, size_t bufferSize) { memcpy(buffer, &encodedSchemeSize, sizeof(encodedSchemeSize)); buffer += sizeof(encodedSchemeSize); - ssize_t writtenSize = serializeSensorScheme(scheme, buffer, bufferSize - (buffer - bufferStart)); + + + SensorScheme schemeForHeader = *scheme; + + struct sensor_config activeConfig; + if (get_sensor_config_status(scheme->id, &activeConfig) == 0 && + activeConfig.sampleRateIndex < + scheme->configOptions.frequencyOptions.frequencyCount) { + + schemeForHeader.configOptions + .frequencyOptions + .defaultFrequencyIndex = + activeConfig.sampleRateIndex; + } + + ssize_t writtenSize = serializeSensorScheme( + &schemeForHeader, + buffer, + bufferSize - (buffer - bufferStart) + ); + if (writtenSize < 0) { return writtenSize; } diff --git a/src/SD_Card/SDLogger/SDLogger.cpp b/src/SD_Card/SDLogger/SDLogger.cpp index 335d4b1b..d3d02bd5 100644 --- a/src/SD_Card/SDLogger/SDLogger.cpp +++ b/src/SD_Card/SDLogger/SDLogger.cpp @@ -92,6 +92,10 @@ void sensor_listener_cb(const struct zbus_channel *chan) { const sensor_msg* msg = (sensor_msg*)zbus_chan_const_msg(chan); if (msg->sd) { + if (!sdlogger.is_active()) { + return; + } + int ret = sdlogger.write_sensor_data(msg->data); if (ret < 0) { LOG_WRN("Failed to enqueue sensor data for SD: %d", ret); diff --git a/src/SensorManager/SensorManager.cpp b/src/SensorManager/SensorManager.cpp index 1eea6daf..82b9e390 100644 --- a/src/SensorManager/SensorManager.cpp +++ b/src/SensorManager/SensorManager.cpp @@ -60,6 +60,7 @@ struct k_thread sensor_publish; static k_tid_t sensor_pub_id; static struct k_work config_work; +static struct k_work_delayable sd_logger_start_work; struct k_work_q sensor_work_q; @@ -67,8 +68,10 @@ K_THREAD_STACK_DEFINE(sensor_publish_thread_stack, CONFIG_SENSOR_PUB_STACK_SIZE) int active_sensors = 0; static const char sensor_manager_auto_off_token[] = "SensorManager"; +static constexpr int SD_LOGGER_START_DEBOUNCE_MS = 300; static void config_work_handler(struct k_work *work); +static void sd_logger_start_work_handler(struct k_work *work); void sensor_chan_update(void *p1, void *p2, void *p3) { int ret; @@ -101,6 +104,7 @@ void init_sensor_manager() { K_PRIO_PREEMPT(CONFIG_SENSOR_PUB_THREAD_PRIO), 0, K_FOREVER); // Thread ist initial suspendiert k_work_init(&config_work, config_work_handler); + k_work_init_delayable(&sd_logger_start_work, sd_logger_start_work_handler); k_poll_signal_init(&sensor_manager_sig); @@ -155,6 +159,7 @@ void stop_sensor_manager() { active_sensors = 0; auto_off_manager.allow(sensor_manager_auto_off_token); + k_work_cancel_delayable(&sd_logger_start_work); k_work_queue_drain(&sensor_work_q, true); @@ -188,6 +193,33 @@ EdgeMlSensor * get_sensor(enum sensor_id id) { } } +static void schedule_sd_logger_start() { + if (sdlogger.is_active() || sd_sensors.empty()) { + return; + } + + (void)k_work_reschedule(&sd_logger_start_work, K_MSEC(SD_LOGGER_START_DEBOUNCE_MS)); +} + +static void sd_logger_start_work_handler(struct k_work *work) { + ARG_UNUSED(work); + + if (sdlogger.is_active() || sd_sensors.empty()) { + return; + } + + const char *recording_name_prefix = get_sensor_recording_name(); + LOG_INF("Starting SDLogger with recording name prefix: %s", recording_name_prefix); + + std::string filename = recording_name_prefix + std::to_string(micros()); + int ret = sdlogger.begin(filename); + if (ret == 0) { + state_indicator.set_sd_state(SD_RECORDING); + } else { + LOG_ERR("Failed to start SDLogger, ret: %d", ret); + } +} + // Worker-Funktion für die Sensor-Konfiguration static void config_work_handler(struct k_work *work) { int ret; @@ -196,6 +228,7 @@ static void config_work_handler(struct k_work *work) { ret = k_msgq_get(&config_queue, &config, K_NO_WAIT); if (ret != 0) { LOG_INF("No config available"); + return; } float sampleRate = getSampleRateForSensorId(config.sensorId, config.sampleRateIndex); @@ -235,21 +268,15 @@ static void config_work_handler(struct k_work *work) { } } + set_sensor_config_status(config); + if (config.storageOptions & DATA_STORAGE) { sd_sensors.insert(config.sensorId); - - if (!sdlogger.is_active()) { - const char *recording_name_prefix = get_sensor_recording_name(); - LOG_INF("Starting SDLogger with recording name prefix: %s", recording_name_prefix); - // Start SDLogger with timestamp-based filename - std::string filename = recording_name_prefix + std::to_string(micros()); - int ret = sdlogger.begin(filename); - if (ret == 0) state_indicator.set_sd_state(SD_RECORDING); - } } else if (sd_sensors.find(config.sensorId) != sd_sensors.end()) { sd_sensors.erase(config.sensorId); if (sd_sensors.empty()) { + k_work_cancel_delayable(&sd_logger_start_work); sdlogger.end(); state_indicator.set_sd_state(SD_IDLE); } @@ -262,7 +289,7 @@ static void config_work_handler(struct k_work *work) { // TODO: if (ble_sensors.empty()) ... } - set_sensor_config_status(config); + schedule_sd_logger_start(); if (active_sensors == 0) stop_sensor_manager(); } @@ -277,4 +304,4 @@ void config_sensor(struct sensor_config * config) { //k_work_queue_drain(&sensor_work_q, true); k_work_submit(&config_work); //k_work_queue_unplug(&sensor_work_q); -} \ No newline at end of file +} diff --git a/src/bluetooth/gatt_services/sensor_service.c b/src/bluetooth/gatt_services/sensor_service.c index 3d821d14..bb2454fc 100644 --- a/src/bluetooth/gatt_services/sensor_service.c +++ b/src/bluetooth/gatt_services/sensor_service.c @@ -1,3 +1,4 @@ +#include #include "sensor_service.h" #include #include @@ -447,6 +448,22 @@ int init_sensor_config_status() { return 0; } +int get_sensor_config_status(uint8_t sensor_id, struct sensor_config *config) +{ + if (config == NULL) { + return -EINVAL; + } + + for (size_t i = 0; i < active_sensor_configs_size; i++) { + if (active_sensor_configs[i].sensorId == sensor_id) { + *config = active_sensor_configs[i]; + return 0; + } + } + + return -ENOENT; +} + int set_sensor_config_status(struct sensor_config config) { LOG_DBG("Setting sensor config status for sensorId: %i", config.sensorId); diff --git a/src/bluetooth/gatt_services/sensor_service.h b/src/bluetooth/gatt_services/sensor_service.h index 239062fe..b4249c67 100644 --- a/src/bluetooth/gatt_services/sensor_service.h +++ b/src/bluetooth/gatt_services/sensor_service.h @@ -35,6 +35,16 @@ int init_sensor_service(); const char *get_sensor_recording_name(); //int send_sensor_data(); //struct sensor_data * data); +/** + * @brief Copy the current runtime configuration for a sensor. + * + * @param sensor_id Sensor to find. + * @param config Destination for the configuration. + * + * @return 0 on success, otherwise a negative errno value. + */ +int get_sensor_config_status(uint8_t sensor_id, struct sensor_config *config); + int set_sensor_config_status(struct sensor_config config); void temp_disable_notifies(bool disable);