Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
22 changes: 21 additions & 1 deletion src/ParseInfo/SensorScheme.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down
4 changes: 4 additions & 0 deletions src/SD_Card/SDLogger/SDLogger.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
49 changes: 38 additions & 11 deletions src/SensorManager/SensorManager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -60,15 +60,18 @@ 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;

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;
Expand Down Expand Up @@ -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);

Expand Down Expand Up @@ -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);

Expand Down Expand Up @@ -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;
Expand All @@ -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);
Expand Down Expand Up @@ -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);
}
Expand All @@ -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();
}
Expand All @@ -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);
}
}
17 changes: 17 additions & 0 deletions src/bluetooth/gatt_services/sensor_service.c
Original file line number Diff line number Diff line change
@@ -1,3 +1,4 @@
#include <errno.h>
#include "sensor_service.h"
#include <zephyr/zbus/zbus.h>
#include <zephyr/kernel.h>
Expand Down Expand Up @@ -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);

Expand Down
10 changes: 10 additions & 0 deletions src/bluetooth/gatt_services/sensor_service.h
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
Loading