From 297161979f514e14a4737e636206a1c3a068a2f5 Mon Sep 17 00:00:00 2001 From: Jason Date: Sun, 25 Jan 2026 16:27:02 -0800 Subject: [PATCH 1/7] Standardized data format to OpenRocket output CSV --- drivers/sensor/sim_baro/sim_baro.c | 152 +++++++++++++++++++++++------ 1 file changed, 124 insertions(+), 28 deletions(-) diff --git a/drivers/sensor/sim_baro/sim_baro.c b/drivers/sensor/sim_baro/sim_baro.c index a0c1344..93f5490 100644 --- a/drivers/sensor/sim_baro/sim_baro.c +++ b/drivers/sensor/sim_baro/sim_baro.c @@ -18,6 +18,62 @@ LOG_MODULE_REGISTER(sim_baro, LOG_LEVEL_INF); #define MAX_LINE_LENGTH 512 #define MAX_CSV_ROWS 10000 +// OpenRocket CSV Column definitions +#define CSV_COL_TIMESTAMP 0 +#define CSV_COL_ALTITUDE 1 +#define CSV_COL_VERTICAL_VELO 2 +#define CSV_COL_VERTICAL_ACCEL 3 +#define CSV_COL_TOTAL_VELO 4 +#define CSV_COL_TOTAL_ACCEL 5 +#define CSV_COL_POS_EAST 6 +#define CSV_COL_POS_NORTH 7 +#define CSV_COL_GPS_LAT_DIST 8 +#define CSV_COL_GPS_LAT_DIR 9 +#define CSV_COL_GPS_LAT_VELO 10 +#define CSV_COL_GPS_LAT_ACCEL 11 +#define CSV_COL_LATITUDE 12 +#define CSV_COL_LONGITUDE 13 +#define CSV_COL_GRAVITY 14 +#define CSV_COL_ANGLE_ATTACK 15 +#define CSV_COL_ROLL_RATE 16 +#define CSV_COL_PITCH_RATE 17 +#define CSV_COL_YAW_RATE 18 +#define CSV_COL_MASS 19 +#define CSV_COL_MOTOR_MASS 20 +#define CSV_COL_LONG_MMOI 21 +#define CSV_COL_ROT_MMOI 22 +#define CSV_COL_CP_LOCATION 23 +#define CSV_COL_CG_LOCATION 24 +#define CSV_COL_STABILITY 25 +#define CSV_COL_MACH_NUMBER 26 +#define CSV_COL_REYNOLDS_NUMBER 27 +#define CSV_COL_THRUST 28 +#define CSV_COL_DRAG 29 +#define CSV_COL_DRAG_COEFF 30 +#define CSV_COL_AXIAL_DRAG_COEFF 31 +#define CSV_COL_FRIC_DRAG_COEFF 32 +#define CSV_COL_PRESSURE_DRAG_COEFF 33 +#define CSV_COL_BASE_DRAG_COEFF 34 +#define CSV_COL_NORM_FORCE_COEFF 35 +#define CSV_COL_PITCH_MOM_COEFF 36 +#define CSV_COL_YAW_MOM_COEFF 37 +#define CSV_COL_SIDE_FORCE_COEFF 38 +#define CSV_COL_ROLL_MOM_COEFF 39 +#define CSV_COL_ROLL_FORCING_COEFF 40 +#define CSV_COL_ROLL_DAMPING_COEFF 41 +#define CSV_COL_PITCH_DAMPING_COEFF 42 +#define CSV_COL_CORIOLIS_ACCEL 43 +#define CSV_COL_REF_LENGTH 44 +#define CSV_COL_REF_AREA 45 +#define CSV_COL_VERTICAL_ORIENT 46 +#define CSV_COL_LATERAL_ORIENT 47 +#define CSV_COL_WIND_SPEED 48 +#define CSV_COL_AIR_TEMP 49 +#define CSV_COL_AIR_PRESSURE 50 +#define CSV_COL_SPEED_OF_SOUND 51 +#define CSV_COL_SIM_TIMESTEP 52 +#define CSV_COL_COMPUTATION_TIME 53 + struct csv_row { int64_t timestamp_ms; float pressure_mbar; // mbar = hPa @@ -45,40 +101,80 @@ struct sim_baro_data { }; -static int parse_csv_line(const char *line, struct csv_row *row) + +// Helper to get Nth field from CSV line +static const char* get_csv_field(const char *line, int field_index, char *buffer, size_t buf_size) { - char buffer[MAX_LINE_LENGTH]; - strncpy(buffer, line, MAX_LINE_LENGTH - 1); - buffer[MAX_LINE_LENGTH - 1] = '\0'; - - char *token; - char *saveptr; - int field = 0; - - token = strtok_r(buffer, ",", &saveptr); - while (token != NULL) { - switch (field) { - case 0: // Timestamp (ms) - row->timestamp_ms = (int64_t)atoll(token); - break; - case 2: // Altitude (m) - row->altitude_m = atof(token); - break; - case 4: // Pressure (mbar) - row->pressure_mbar = atof(token) / 100.0f; // Convert cmbar to mbar - break; - case 5: // Barom. Temp (0.01 C) - need to divide by 100 - row->temperature_c = atof(token) / 100.0f; - break; + const char *start = line; + const char *end; + int current_field = 0; + + // Skip to the desired field + while (current_field < field_index && *start) { + if (*start == ',') { + current_field++; } - field++; - token = strtok_r(NULL, ",", &saveptr); + start++; + } + + if (current_field != field_index) { + return NULL; // Field not found + } + + // Find the end of this field + end = start; + while (*end && *end != ',' && *end != '\n' && *end != '\r') { + end++; + } + + // Copy to buffer + size_t len = end - start; + if (len >= buf_size) { + len = buf_size - 1; } - // Validate we got the required fields - return (field >= 6) ? 0 : -1; + strncpy(buffer, start, len); + buffer[len] = '\0'; + + return buffer; +} + + + +// Helper macro to extract and parse a field +#define GET_CSV_INT64(line, col, dest) do { \ + char _buf[64]; \ + if (!get_csv_field(line, col, _buf, sizeof(_buf))) return -1; \ + dest = atoll(_buf); \ +} while(0) + +// Helper macro to extract and parse a field +#define GET_CSV_FLOAT(line, col, dest) do { \ + char _buf[64]; \ + if (!get_csv_field(line, col, _buf, sizeof(_buf))) return -1; \ + dest = atof(_buf); \ +} while(0) + +static int parse_csv_line(const char *line, struct csv_row *row) +{ + float pressure_raw, temp_raw, timestamp_raw, altitude_raw; + + GET_CSV_FLOAT(line, CSV_COL_TIMESTAMP, timestamp_raw); + GET_CSV_FLOAT(line, CSV_COL_ALTITUDE, altitude_raw); + GET_CSV_FLOAT(line, CSV_COL_AIR_PRESSURE, pressure_raw); + GET_CSV_FLOAT(line, CSV_COL_AIR_TEMP, temp_raw); + + // Apply unit conversions + row->timestamp_ms = timestamp_raw * 1000.0f; // Convert from seconds to milliseconds + row->altitude_m = altitude_raw; + row->pressure_mbar = pressure_raw; // mbar = hPa + row->temperature_c = temp_raw; + + return 0; } + + static int load_csv_data(struct sim_baro_data *data) { LOG_INF("═══════════════════════════════════════════════"); From 22fe497c38750478703f3bd22038e761ccdf3a47 Mon Sep 17 00:00:00 2001 From: Jason Date: Sun, 25 Jan 2026 22:16:00 -0800 Subject: [PATCH 2/7] Ignore data files and OpenRocket --- .gitignore | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index 2301de3..df47cc9 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,8 @@ **/build/ .DS_Store twister-out* -.cache \ No newline at end of file +.cache + +data/ +*.jar +*.ork \ No newline at end of file From 5c6dacda74e11fb92a937b671a120ef17416b1cc Mon Sep 17 00:00:00 2001 From: Jason Date: Sun, 25 Jan 2026 22:58:01 -0800 Subject: [PATCH 3/7] Add integration test steps to README --- README.md | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/README.md b/README.md index 8b95456..6924032 100644 --- a/README.md +++ b/README.md @@ -34,6 +34,29 @@ TODO - Zephyr Documentation: https://docs.zephyrproject.org/latest/index.html - Highly recommend watching this tutorial series (Videos 9, 10, and 11 shouldnt be necessary for this project): https://www.youtube.com/playlist?list=PLEBQazB0HUyTmK2zdwhaf8bLwuEaDH-52 +## Integration Testing +### Zephyr's Native Sim +#### Getting the data +1. Follow the installation instruction for OpenRocket: https://openrocket.readthedocs.io/en/latest/setup/installation.html +2. Boot up OpenRocket and load the .ork file +3. Under Flight Simulations, open a previous simulation by double clicking on the row +4. Under Export Data, click "select all" and ensure that only the "Include field descriptions" option is checked. +5. Click Export and save your csv file + +#### Running the integration test +1. Ensure you're in the virtual environment using inside `FALCON/`: + - `source .venv/bin/activate` (MacOS/Linux) or `.venv\Scripts\activate.bat` (Windows) +2. Build FALCON using Zephyr's built in native_sim board: + - `west build -b native_sim/native/64 app/apps/rockets/cloudburst -- -DDTC_OVERLAY_FILE="$(pwd)/boards/native_sim.overlay" -DDATA_FILE="$(pwd)//.csv"` + - Replace `PATH` and `DATA_FILE` with the location and name of your OpenRocket csv file +3. Run the integration test: + - For a realtime test, use `west build -t run` + - To run at the maximum speed, use `./app/build/zephyr/zephyr.exe --no-rt` + - To run at a custom speed, use `./app/build/zephyr/zephyr.exe --rt-ratio=2` + +### QEMU (WIP) + + ## Debugging ### Terminal debugging: *Should work out of the box provided your zephyr environment is set up correctly* From c17b0b715a3ed4a66533378e54d9ba3d56a49953 Mon Sep 17 00:00:00 2001 From: Jason Date: Sun, 25 Jan 2026 22:58:01 -0800 Subject: [PATCH 4/7] Add integration test steps to README --- README.md | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/README.md b/README.md index 8b95456..6924032 100644 --- a/README.md +++ b/README.md @@ -34,6 +34,29 @@ TODO - Zephyr Documentation: https://docs.zephyrproject.org/latest/index.html - Highly recommend watching this tutorial series (Videos 9, 10, and 11 shouldnt be necessary for this project): https://www.youtube.com/playlist?list=PLEBQazB0HUyTmK2zdwhaf8bLwuEaDH-52 +## Integration Testing +### Zephyr's Native Sim +#### Getting the data +1. Follow the installation instruction for OpenRocket: https://openrocket.readthedocs.io/en/latest/setup/installation.html +2. Boot up OpenRocket and load the .ork file +3. Under Flight Simulations, open a previous simulation by double clicking on the row +4. Under Export Data, click "select all" and ensure that only the "Include field descriptions" option is checked. +5. Click Export and save your csv file + +#### Running the integration test +1. Ensure you're in the virtual environment using inside `FALCON/`: + - `source .venv/bin/activate` (MacOS/Linux) or `.venv\Scripts\activate.bat` (Windows) +2. Build FALCON using Zephyr's built in native_sim board: + - `west build -b native_sim/native/64 app/apps/rockets/cloudburst -- -DDTC_OVERLAY_FILE="$(pwd)/boards/native_sim.overlay" -DDATA_FILE="$(pwd)//.csv"` + - Replace `PATH` and `DATA_FILE` with the location and name of your OpenRocket csv file +3. Run the integration test: + - For a realtime test, use `west build -t run` + - To run at the maximum speed, use `./app/build/zephyr/zephyr.exe --no-rt` + - To run at a custom speed, use `./app/build/zephyr/zephyr.exe --rt-ratio=2` + +### QEMU (WIP) + + ## Debugging ### Terminal debugging: *Should work out of the box provided your zephyr environment is set up correctly* From 9255a77921e7f3c6a56f0baaa2e0a2e4c0878b84 Mon Sep 17 00:00:00 2001 From: Jason Date: Tue, 27 Jan 2026 11:08:54 -0800 Subject: [PATCH 5/7] Added pyro board SPI support for native_sim --- boards/native_sim.overlay | 16 ++++++++++++++++ drivers/sensor/sim_pyro/CMakeLists.txt | 2 ++ drivers/sensor/sim_pyro/Kconfig | 3 +++ drivers/sensor/sim_pyro/sim_pyro.c | 0 dts/bindings/sensor/zephyr,sim-pyro.yaml | 21 +++++++++++++++++++++ 5 files changed, 42 insertions(+) create mode 100644 drivers/sensor/sim_pyro/CMakeLists.txt create mode 100644 drivers/sensor/sim_pyro/Kconfig create mode 100644 drivers/sensor/sim_pyro/sim_pyro.c create mode 100644 dts/bindings/sensor/zephyr,sim-pyro.yaml diff --git a/boards/native_sim.overlay b/boards/native_sim.overlay index 730045d..4f19ad2 100644 --- a/boards/native_sim.overlay +++ b/boards/native_sim.overlay @@ -35,11 +35,27 @@ sector-count = <2048>; /* 1 MB */ }; + spi_pyro: spi@40000000 { + compatible = "vnd,spi"; + reg = <0x40000000 0x100>; + #address-cells = <1>; + #size-cells = <0>; + status = "okay"; + + sim_pyro0: sim_pyro_0@0 { + compatible = "zephyr,sim-pyro"; + reg = <0>; + spi-max-frequency = <1000000>; + status = "okay"; + }; + }; + aliases { baro0 = &sim_baro0; baro1 = &sim_baro1; accel0 = &sim_accel0; gyro0 = &sim_gyro0; + pyro0 = &sim_pyro0; sdmmc1 = &sim_sdmmc1; ramdisk0 = &sim_ramdisk0; }; diff --git a/drivers/sensor/sim_pyro/CMakeLists.txt b/drivers/sensor/sim_pyro/CMakeLists.txt new file mode 100644 index 0000000..e598bce --- /dev/null +++ b/drivers/sensor/sim_pyro/CMakeLists.txt @@ -0,0 +1,2 @@ +zephyr_library() +zephyr_library_sources_ifdef(CONFIG_SIM_PYRO sim_pyro.c) \ No newline at end of file diff --git a/drivers/sensor/sim_pyro/Kconfig b/drivers/sensor/sim_pyro/Kconfig new file mode 100644 index 0000000..44ca26f --- /dev/null +++ b/drivers/sensor/sim_pyro/Kconfig @@ -0,0 +1,3 @@ +config SIM_PYRO + bool "Simulated Pyro Board" + depends on DT_HAS_ZEPHYR_SIM_PYRO_ENABLED \ No newline at end of file diff --git a/drivers/sensor/sim_pyro/sim_pyro.c b/drivers/sensor/sim_pyro/sim_pyro.c new file mode 100644 index 0000000..e69de29 diff --git a/dts/bindings/sensor/zephyr,sim-pyro.yaml b/dts/bindings/sensor/zephyr,sim-pyro.yaml new file mode 100644 index 0000000..c412f42 --- /dev/null +++ b/dts/bindings/sensor/zephyr,sim-pyro.yaml @@ -0,0 +1,21 @@ +description: Simulated Pyro board for native_sim +compatible: "zephyr,sim-pyro" +include: [sensor-device.yaml, spi-device.yaml] + +properties: + spi-max-frequency: + type: int + required: true + description: Maximum SPI frequency in Hz + + duplex: + type: int + description: SPI duplex mode (full-duplex, half-duplex, etc.) + + frame-format: + type: int + description: SPI frame format (Motorola, TI, etc.) + + spi-interframe-delay-ns: + type: int + description: Inter-frame delay in nanoseconds \ No newline at end of file From fe728eebdd5ea9e254a80083dfb858496f269026 Mon Sep 17 00:00:00 2001 From: Jason Date: Wed, 28 Jan 2026 20:07:46 -0800 Subject: [PATCH 6/7] Initial CSV refactoring --- apps/rockets/cloudburst/prj.conf | 5 + drivers/sensor/CMakeLists.txt | 5 +- drivers/sensor/Kconfig | 2 + drivers/sensor/sim_baro/CMakeLists.txt | 5 +- drivers/sensor/sim_baro/sim_baro.c | 396 ++++------------------- drivers/sensor/sim_common/CMakeLists.txt | 3 + drivers/sensor/sim_common/Kconfig | 5 + drivers/sensor/sim_common/sim_csv.c | 306 ++++++++++++++++++ drivers/sensor/sim_common/sim_csv.h | 145 +++++++++ 9 files changed, 528 insertions(+), 344 deletions(-) create mode 100644 drivers/sensor/sim_common/CMakeLists.txt create mode 100644 drivers/sensor/sim_common/Kconfig create mode 100644 drivers/sensor/sim_common/sim_csv.c create mode 100644 drivers/sensor/sim_common/sim_csv.h diff --git a/apps/rockets/cloudburst/prj.conf b/apps/rockets/cloudburst/prj.conf index 7b050b1..e138450 100644 --- a/apps/rockets/cloudburst/prj.conf +++ b/apps/rockets/cloudburst/prj.conf @@ -1,11 +1,16 @@ # Enable simulated sensors for native_sim +CONFIG_SIM_SENSORS=y + +# Enable individual simulated sensors CONFIG_SIM_BARO=y CONFIG_SIM_ACCEL=y CONFIG_SIM_GYRO=y CONFIG_SIM_SDMMC=y +CONFIG_SIM_PYRO=y # Enable fake sensors for native_sim CONFIG_EMUL=y +CONFIG_SPI_EMUL=n CONFIG_SENSOR_SHELL=y # Enable the random number generator for native_sim diff --git a/drivers/sensor/CMakeLists.txt b/drivers/sensor/CMakeLists.txt index 2763f00..edd324a 100644 --- a/drivers/sensor/CMakeLists.txt +++ b/drivers/sensor/CMakeLists.txt @@ -1,5 +1,8 @@ add_subdirectory_ifdef(CONFIG_MS5611 ms5611) + +add_subdirectory_ifdef(CONFIG_SIM_SENSORS sim_common) add_subdirectory_ifdef(CONFIG_SIM_BARO sim_baro) add_subdirectory_ifdef(CONFIG_SIM_ACCEL sim_accel) add_subdirectory_ifdef(CONFIG_SIM_GYRO sim_gyro) -add_subdirectory_ifdef(CONFIG_SIM_SDMMC sim_sdmmc) \ No newline at end of file +add_subdirectory_ifdef(CONFIG_SIM_SDMMC sim_sdmmc) +add_subdirectory_ifdef(CONFIG_SIM_PYRO sim_pyro) \ No newline at end of file diff --git a/drivers/sensor/Kconfig b/drivers/sensor/Kconfig index 6be37f3..a7611b5 100644 --- a/drivers/sensor/Kconfig +++ b/drivers/sensor/Kconfig @@ -1,7 +1,9 @@ if SENSOR rsource "ms5611/Kconfig" +rsource "sim_common/Kconfig" rsource "sim_baro/Kconfig" rsource "sim_accel/Kconfig" rsource "sim_gyro/Kconfig" rsource "sim_sdmmc/Kconfig" +rsource "sim_pyro/Kconfig" endif # SENSOR \ No newline at end of file diff --git a/drivers/sensor/sim_baro/CMakeLists.txt b/drivers/sensor/sim_baro/CMakeLists.txt index d359075..14490fa 100644 --- a/drivers/sensor/sim_baro/CMakeLists.txt +++ b/drivers/sensor/sim_baro/CMakeLists.txt @@ -1,2 +1,5 @@ zephyr_library() -zephyr_library_sources_ifdef(CONFIG_SIM_BARO sim_baro.c) \ No newline at end of file +zephyr_library_sources_ifdef(CONFIG_SIM_BARO sim_baro.c) + +zephyr_include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../sim_common) +zephyr_library_sources(${CMAKE_CURRENT_SOURCE_DIR}/../sim_common/sim_csv.c) \ No newline at end of file diff --git a/drivers/sensor/sim_baro/sim_baro.c b/drivers/sensor/sim_baro/sim_baro.c index 93f5490..a0902a3 100644 --- a/drivers/sensor/sim_baro/sim_baro.c +++ b/drivers/sensor/sim_baro/sim_baro.c @@ -1,85 +1,18 @@ #define DT_DRV_COMPAT zephyr_sim_baro +#include "sim_csv.h" #include #include #include #include #include #include -#include -#include -#include LOG_MODULE_REGISTER(sim_baro, LOG_LEVEL_INF); #ifndef DATA_FILE #define DATA_FILE "" #endif -#define MAX_LINE_LENGTH 512 -#define MAX_CSV_ROWS 10000 - -// OpenRocket CSV Column definitions -#define CSV_COL_TIMESTAMP 0 -#define CSV_COL_ALTITUDE 1 -#define CSV_COL_VERTICAL_VELO 2 -#define CSV_COL_VERTICAL_ACCEL 3 -#define CSV_COL_TOTAL_VELO 4 -#define CSV_COL_TOTAL_ACCEL 5 -#define CSV_COL_POS_EAST 6 -#define CSV_COL_POS_NORTH 7 -#define CSV_COL_GPS_LAT_DIST 8 -#define CSV_COL_GPS_LAT_DIR 9 -#define CSV_COL_GPS_LAT_VELO 10 -#define CSV_COL_GPS_LAT_ACCEL 11 -#define CSV_COL_LATITUDE 12 -#define CSV_COL_LONGITUDE 13 -#define CSV_COL_GRAVITY 14 -#define CSV_COL_ANGLE_ATTACK 15 -#define CSV_COL_ROLL_RATE 16 -#define CSV_COL_PITCH_RATE 17 -#define CSV_COL_YAW_RATE 18 -#define CSV_COL_MASS 19 -#define CSV_COL_MOTOR_MASS 20 -#define CSV_COL_LONG_MMOI 21 -#define CSV_COL_ROT_MMOI 22 -#define CSV_COL_CP_LOCATION 23 -#define CSV_COL_CG_LOCATION 24 -#define CSV_COL_STABILITY 25 -#define CSV_COL_MACH_NUMBER 26 -#define CSV_COL_REYNOLDS_NUMBER 27 -#define CSV_COL_THRUST 28 -#define CSV_COL_DRAG 29 -#define CSV_COL_DRAG_COEFF 30 -#define CSV_COL_AXIAL_DRAG_COEFF 31 -#define CSV_COL_FRIC_DRAG_COEFF 32 -#define CSV_COL_PRESSURE_DRAG_COEFF 33 -#define CSV_COL_BASE_DRAG_COEFF 34 -#define CSV_COL_NORM_FORCE_COEFF 35 -#define CSV_COL_PITCH_MOM_COEFF 36 -#define CSV_COL_YAW_MOM_COEFF 37 -#define CSV_COL_SIDE_FORCE_COEFF 38 -#define CSV_COL_ROLL_MOM_COEFF 39 -#define CSV_COL_ROLL_FORCING_COEFF 40 -#define CSV_COL_ROLL_DAMPING_COEFF 41 -#define CSV_COL_PITCH_DAMPING_COEFF 42 -#define CSV_COL_CORIOLIS_ACCEL 43 -#define CSV_COL_REF_LENGTH 44 -#define CSV_COL_REF_AREA 45 -#define CSV_COL_VERTICAL_ORIENT 46 -#define CSV_COL_LATERAL_ORIENT 47 -#define CSV_COL_WIND_SPEED 48 -#define CSV_COL_AIR_TEMP 49 -#define CSV_COL_AIR_PRESSURE 50 -#define CSV_COL_SPEED_OF_SOUND 51 -#define CSV_COL_SIM_TIMESTEP 52 -#define CSV_COL_COMPUTATION_TIME 53 - -struct csv_row { - int64_t timestamp_ms; - float pressure_mbar; // mbar = hPa - float temperature_c; - float altitude_m; -}; struct sim_baro_data { float altitude_m; @@ -88,298 +21,86 @@ struct sim_baro_data { float temperature_c; int64_t last_ms; - // CSV playback data - struct csv_row *csv_data; - size_t csv_row_count; - size_t csv_current_index; - int64_t csv_start_time_ms; - int64_t csv_first_timestamp; - bool csv_loaded; - - // Logging control - uint32_t sample_count; + struct sim_csv_context csv_ctx; }; - -// Helper to get Nth field from CSV line -static const char* get_csv_field(const char *line, int field_index, char *buffer, size_t buf_size) +static void baro_copy_to_sensor(void *sensor_data, const struct sim_csv_row *csv_row) { - const char *start = line; - const char *end; - int current_field = 0; - - // Skip to the desired field - while (current_field < field_index && *start) { - if (*start == ',') { - current_field++; - } - start++; - } + struct sim_baro_data *data = (struct sim_baro_data *)sensor_data; - if (current_field != field_index) { - return NULL; // Field not found - } - - // Find the end of this field - end = start; - while (*end && *end != ',' && *end != '\n' && *end != '\r') { - end++; - } - - // Copy to buffer - size_t len = end - start; - if (len >= buf_size) { - len = buf_size - 1; - } - - strncpy(buffer, start, len); - buffer[len] = '\0'; - - return buffer; + data->pressure_hpa = csv_row->fields[CSV_COL_AIR_PRESSURE]; + data->temperature_c = csv_row->fields[CSV_COL_AIR_TEMP]; + data->altitude_m = csv_row->fields[CSV_COL_ALTITUDE]; } - - -// Helper macro to extract and parse a field -#define GET_CSV_INT64(line, col, dest) do { \ - char _buf[64]; \ - if (!get_csv_field(line, col, _buf, sizeof(_buf))) return -1; \ - dest = atoll(_buf); \ -} while(0) - -// Helper macro to extract and parse a field -#define GET_CSV_FLOAT(line, col, dest) do { \ - char _buf[64]; \ - if (!get_csv_field(line, col, _buf, sizeof(_buf))) return -1; \ - dest = atof(_buf); \ -} while(0) - -static int parse_csv_line(const char *line, struct csv_row *row) +static void baro_interpolate(void *sensor_data, + const struct sim_csv_row *row_curr, + const struct sim_csv_row *row_next, + float alpha) { - float pressure_raw, temp_raw, timestamp_raw, altitude_raw; + struct sim_baro_data *data = (struct sim_baro_data *)sensor_data; - GET_CSV_FLOAT(line, CSV_COL_TIMESTAMP, timestamp_raw); - GET_CSV_FLOAT(line, CSV_COL_ALTITUDE, altitude_raw); - GET_CSV_FLOAT(line, CSV_COL_AIR_PRESSURE, pressure_raw); - GET_CSV_FLOAT(line, CSV_COL_AIR_TEMP, temp_raw); + data->pressure_hpa = row_curr->fields[CSV_COL_AIR_PRESSURE] + + alpha * (row_next->fields[CSV_COL_AIR_PRESSURE] - + row_curr->fields[CSV_COL_AIR_PRESSURE]); - // Apply unit conversions - row->timestamp_ms = timestamp_raw * 1000.0f; // Convert from seconds to milliseconds - row->altitude_m = altitude_raw; - row->pressure_mbar = pressure_raw; // mbar = hPa - row->temperature_c = temp_raw; + data->temperature_c = row_curr->fields[CSV_COL_AIR_TEMP] + + alpha * (row_next->fields[CSV_COL_AIR_TEMP] - + row_curr->fields[CSV_COL_AIR_TEMP]); - return 0; + data->altitude_m = row_curr->fields[CSV_COL_ALTITUDE] + + alpha * (row_next->fields[CSV_COL_ALTITUDE] - + row_curr->fields[CSV_COL_ALTITUDE]); } - - -static int load_csv_data(struct sim_baro_data *data) +static void baro_log_first_row(const struct sim_csv_row *row) { - LOG_INF("═══════════════════════════════════════════════"); - LOG_INF(" CSV DATA LOADING"); - LOG_INF("═══════════════════════════════════════════════"); - LOG_INF("Attempting to open: %s", DATA_FILE); - - // In Zephyr native_sim, we can use standard C file I/O - FILE *fp = fopen(DATA_FILE, "r"); - if (!fp) { - LOG_ERR("Failed to open CSV file: %s", DATA_FILE); - LOG_WRN("Falling back to SYNTHETIC data mode"); - return -1; - } - - LOG_INF("File opened successfully"); - - // Allocate memory for CSV data - data->csv_data = malloc(sizeof(struct csv_row) * MAX_CSV_ROWS); - if (!data->csv_data) { - fclose(fp); - LOG_ERR("Failed to allocate memory for CSV data"); - LOG_WRN("Falling back to SYNTHETIC data mode"); - return -ENOMEM; - } - - LOG_INF("Memory allocated for %d rows", MAX_CSV_ROWS); - - char line[MAX_LINE_LENGTH]; - data->csv_row_count = 0; - - // Skip header line - if (!fgets(line, sizeof(line), fp)) { - fclose(fp); - k_free(data->csv_data); - LOG_ERR("Empty CSV file"); - LOG_WRN("Falling back to SYNTHETIC data mode"); - return -1; - } - - LOG_INF("Header line read"); - - // Read data rows - while (data->csv_row_count < MAX_CSV_ROWS && fgets(line, sizeof(line), fp)) { - struct csv_row *row = &data->csv_data[data->csv_row_count]; - - if (parse_csv_line(line, row) == 0) { - // Store the first timestamp for relative time calculation - if (data->csv_row_count == 0) { - data->csv_first_timestamp = row->timestamp_ms; - LOG_INF("First data point: t=%lld ms, p=%.2f mbar, alt=%.2f m, T=%.2f C", - row->timestamp_ms, - (double)row->pressure_mbar, - (double)row->altitude_m, - (double)row->temperature_c); - } - data->csv_row_count++; - } - } - - fclose(fp); - - if (data->csv_row_count == 0) { - k_free(data->csv_data); - LOG_ERR("No valid data rows in CSV"); - LOG_WRN("Falling back to SYNTHETIC data mode"); - return -1; - } - - data->csv_loaded = true; - - struct csv_row *last_row = &data->csv_data[data->csv_row_count - 1]; - int64_t duration_ms = last_row->timestamp_ms - data->csv_first_timestamp; - - LOG_INF("═══════════════════════════════════════════════"); - LOG_INF(" CSV LOAD SUCCESSFUL"); - LOG_INF("═══════════════════════════════════════════════"); - LOG_INF("Rows loaded: %zu", data->csv_row_count); - LOG_INF("Time range: %lld to %lld ms", - data->csv_data[0].timestamp_ms, - last_row->timestamp_ms); - LOG_INF("Duration: %.2f seconds", (double)duration_ms / 1000.0); - LOG_INF("Altitude range: %.2f to %.2f m", - (double)data->csv_data[0].altitude_m, - (double)last_row->altitude_m); - LOG_INF("Mode: CSV PLAYBACK MODE"); - LOG_INF("═══════════════════════════════════════════════"); - - return 0; + LOG_INF("First data point: t=%.3f s, p=%.2f hPa, alt=%.2f m, T=%.2f C", + (double)row->fields[CSV_COL_TIMESTAMP], + (double)row->fields[CSV_COL_AIR_PRESSURE], + (double)row->fields[CSV_COL_ALTITUDE], + (double)row->fields[CSV_COL_AIR_TEMP]); } -static void interpolate_csv_data(struct sim_baro_data *data, int64_t now_ms) +static void baro_log_summary(const struct sim_csv_row *first_row, + const struct sim_csv_row *last_row, + size_t row_count) { - if (!data->csv_loaded || data->csv_row_count == 0) { - return; - } - - // Calculate elapsed time since start (in simulation time) - int64_t elapsed_ms = now_ms - data->csv_start_time_ms; - - // Map to CSV timestamp (relative to first CSV timestamp) - int64_t target_timestamp = data->csv_first_timestamp + elapsed_ms; - - // Find the two surrounding data points - size_t i = data->csv_current_index; - - // Advance index if needed - while (i < data->csv_row_count - 1 && - data->csv_data[i + 1].timestamp_ms <= target_timestamp) { - i++; - } - - // Log when we advance to a new index - if (i != data->csv_current_index) { - LOG_DBG("CSV index advanced: %zu -> %zu (CSV time: %lld ms)", - data->csv_current_index, i, - data->csv_data[i].timestamp_ms); - } - - data->csv_current_index = i; - - // Handle edge cases - if (i == 0 && target_timestamp < data->csv_data[0].timestamp_ms) { - // Before first data point, use first row - struct csv_row *row = &data->csv_data[0]; - data->pressure_hpa = row->pressure_mbar; // mbar = hPa - data->temperature_c = row->temperature_c; - data->altitude_m = row->altitude_m; - - if (data->sample_count % 100 == 0) { - LOG_DBG("Using first CSV row (before start time)"); - } - return; - } - - if (i >= data->csv_row_count - 1) { - // Past end of data, use last row - struct csv_row *row = &data->csv_data[data->csv_row_count - 1]; - data->pressure_hpa = row->pressure_mbar; - data->temperature_c = row->temperature_c; - data->altitude_m = row->altitude_m; - - if (data->sample_count == 0 || (data->sample_count % 100 == 0)) { - LOG_WRN("End of CSV data reached - holding last values"); - } - return; - } - - // Interpolate between current and next point - struct csv_row *curr = &data->csv_data[i]; - struct csv_row *next = &data->csv_data[i + 1]; - - int64_t dt = next->timestamp_ms - curr->timestamp_ms; - if (dt <= 0) dt = 1; - - float alpha = (float)(target_timestamp - curr->timestamp_ms) / (float)dt; - if (alpha < 0.0f) alpha = 0.0f; - if (alpha > 1.0f) alpha = 1.0f; - - // Linear interpolation - data->pressure_hpa = curr->pressure_mbar + alpha * (next->pressure_mbar - curr->pressure_mbar); - data->temperature_c = curr->temperature_c + alpha * (next->temperature_c - curr->temperature_c); - data->altitude_m = curr->altitude_m + alpha * (next->altitude_m - curr->altitude_m); - - // Periodic detailed logging (every 50 samples) - if (data->sample_count % 50 == 0) { - LOG_INF("CSV: idx=%zu/%zu | t=%lld ms | p=%.2f hPa | alt=%.2f m | T=%.2f C | α=%.3f", - i, data->csv_row_count - 1, - target_timestamp, - (double)data->pressure_hpa, - (double)data->altitude_m, - (double)data->temperature_c, - (double)alpha); - } + LOG_INF("Altitude range: %.2f to %.2f m", + (double)first_row->fields[CSV_COL_ALTITUDE], + (double)last_row->fields[CSV_COL_ALTITUDE]); } +static const struct sim_csv_config baro_csv_config = { + .filename = DATA_FILE, + .sensor_name = "BARO", + .copy_to_sensor = baro_copy_to_sensor, + .interpolate = baro_interpolate, + .log_first_row = baro_log_first_row, + .log_summary = baro_log_summary, +}; + static int sim_baro_sample_fetch(const struct device *dev, enum sensor_channel chan) { struct sim_baro_data *data = dev->data; - int64_t now = k_uptime_get(); float dt = (now - data->last_ms) / 1000.0f; // Initialize on first call if (data->last_ms == 0) { data->last_ms = now; - data->sample_count = 0; LOG_INF("═══════════════════════════════════════════════"); LOG_INF(" SIM_BARO INITIALIZATION"); LOG_INF("═══════════════════════════════════════════════"); - // Load CSV data if DATA_FILE is defined + // Try to load CSV data if (strlen(DATA_FILE) > 0) { LOG_INF("DATA_FILE defined: \"%s\"", DATA_FILE); - if (load_csv_data(data) == 0) { - data->csv_start_time_ms = now; - data->csv_current_index = 0; - - // Initialize with first CSV values - struct csv_row *first = &data->csv_data[0]; - data->pressure_hpa = first->pressure_mbar; - data->temperature_c = first->temperature_c; - data->altitude_m = first->altitude_m; + if (sim_csv_load(&data->csv_ctx, &baro_csv_config) == 0) { + sim_csv_init_playback(&data->csv_ctx, data, now); } } else { LOG_INF("DATA_FILE not defined (empty string)"); @@ -393,43 +114,36 @@ static int sim_baro_sample_fetch(const struct device *dev, data->temperature_c = 20.0f; } - return 0; // Now returns with valid data + return 0; } data->last_ms = now; - data->sample_count++; - if (dt <= 0) dt = 0.02f; - if (strlen(DATA_FILE) > 0 && data->csv_loaded) { + if (data->csv_ctx.csv_loaded) { // Use CSV data - interpolate_csv_data(data, now); + sim_csv_update(&data->csv_ctx, data, now); } else { - // Synthetic data + // Synthetic data mode data->velocity_mps += 0.1f * dt; - data->altitude_m += data->velocity_mps * dt; + data->altitude_m += data->velocity_mps * dt; - /* Add noise */ float noise = ((int32_t)sys_rand32_get() % 1000) / 1000.0f - 0.5f; data->altitude_m += noise * 0.05f; - /* Convert altitude → pressure (ISA approx), result in hPa */ float pressure_pa = 101325.0f * powf(1.0f - (data->altitude_m / 44330.0f), 5.255f); data->pressure_hpa = pressure_pa / 100.0f; - data->temperature_c = 20.0f; - // Periodic logging for synthetic mode (every 50 samples) - if (data->sample_count % 50 == 0) { + if (data->csv_ctx.sample_count % 50 == 0) { LOG_INF("SYN: p=%.2f hPa | alt=%.2f m | v=%.2f m/s | T=%.2f C", - (double)data->pressure_hpa, - (double)data->altitude_m, - (double)data->velocity_mps, - (double)data->temperature_c); + (double)data->pressure_hpa, (double)data->altitude_m, + (double)data->velocity_mps, (double)data->temperature_c); } + data->csv_ctx.sample_count++; } return 0; @@ -461,8 +175,6 @@ static const struct sensor_driver_api sim_baro_api = { #define SIM_BARO_INIT(inst) \ static struct sim_baro_data sim_baro_data_##inst = { \ .last_ms = 0, \ - .csv_loaded = false, \ - .sample_count = 0, \ .pressure_hpa = 1013.25f, \ .temperature_c = 20.0f, \ .altitude_m = 0.0f, \ diff --git a/drivers/sensor/sim_common/CMakeLists.txt b/drivers/sensor/sim_common/CMakeLists.txt new file mode 100644 index 0000000..ffc4e9b --- /dev/null +++ b/drivers/sensor/sim_common/CMakeLists.txt @@ -0,0 +1,3 @@ +zephyr_library() +zephyr_library_sources(sim_csv.c) +zephyr_include_directories(${CMAKE_CURRENT_SOURCE_DIR}) \ No newline at end of file diff --git a/drivers/sensor/sim_common/Kconfig b/drivers/sensor/sim_common/Kconfig new file mode 100644 index 0000000..f908112 --- /dev/null +++ b/drivers/sensor/sim_common/Kconfig @@ -0,0 +1,5 @@ +config SIM_SENSORS + bool "Simulated sensors for native_sim" + depends on DT_HAS_ZEPHYR_SIM_SENSORS_ENABLED + help + Enable simulated sensors for Zephyr's native sim \ No newline at end of file diff --git a/drivers/sensor/sim_common/sim_csv.c b/drivers/sensor/sim_common/sim_csv.c new file mode 100644 index 0000000..992750a --- /dev/null +++ b/drivers/sensor/sim_common/sim_csv.c @@ -0,0 +1,306 @@ +#include "sim_csv.h" +#include +#include +#include +#include +#include +#include + +LOG_MODULE_REGISTER(sim_csv, LOG_LEVEL_INF); + +#define MAX_LINE_LENGTH 4096 + +/** + * Parse entire CSV line into float array using sscanf + * Returns 0 on success, -1 on error + */ +static int parse_csv_row(const char *line, struct sim_csv_row *row) +{ + int result = sscanf(line, + "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f," // 0-9 + "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f," // 10-19 + "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f," // 20-29 + "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f," // 30-39 + "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f," // 40-49 + "%f,%f,%f,%f", // 50-53 + &row->fields[0], &row->fields[1], &row->fields[2], &row->fields[3], + &row->fields[4], &row->fields[5], &row->fields[6], &row->fields[7], + &row->fields[8], &row->fields[9], &row->fields[10], &row->fields[11], + &row->fields[12], &row->fields[13], &row->fields[14], &row->fields[15], + &row->fields[16], &row->fields[17], &row->fields[18], &row->fields[19], + &row->fields[20], &row->fields[21], &row->fields[22], &row->fields[23], + &row->fields[24], &row->fields[25], &row->fields[26], &row->fields[27], + &row->fields[28], &row->fields[29], &row->fields[30], &row->fields[31], + &row->fields[32], &row->fields[33], &row->fields[34], &row->fields[35], + &row->fields[36], &row->fields[37], &row->fields[38], &row->fields[39], + &row->fields[40], &row->fields[41], &row->fields[42], &row->fields[43], + &row->fields[44], &row->fields[45], &row->fields[46], &row->fields[47], + &row->fields[48], &row->fields[49], &row->fields[50], &row->fields[51], + &row->fields[52], &row->fields[53] + ); + + return (result == CSV_NUM_COLUMNS) ? 0 : -1; +} + +/** + * Read next CSV row from file + * Returns 0 on success, -1 on EOF or error + */ +static int read_next_row(FILE *fp, struct sim_csv_row *row_out) +{ + char line[MAX_LINE_LENGTH]; + + if (!fgets(line, sizeof(line), fp)) { + return -1; // EOF or error + } + + return parse_csv_row(line, row_out); +} + +/** + * Scan entire file to get metadata + */ +static int scan_csv_metadata(FILE *fp, struct sim_csv_context *ctx) +{ + char line[MAX_LINE_LENGTH]; + struct sim_csv_row temp_row; + + ctx->csv_row_count = 0; + bool first_row = true; + + // Skip header + if (!fgets(line, sizeof(line), fp)) { + return -EINVAL; + } + + // Scan all data rows + while (fgets(line, sizeof(line), fp)) { + if (parse_csv_row(line, &temp_row) == 0) { + int64_t timestamp = sim_csv_get_timestamp_ms(&temp_row); + + if (first_row) { + ctx->csv_first_timestamp = timestamp; + + if (ctx->config->log_first_row) { + ctx->config->log_first_row(&temp_row); + } + + first_row = false; + } + + ctx->csv_last_timestamp = timestamp; + ctx->csv_row_count++; + } + } + + // Rewind to start of file + rewind(fp); + + // Skip header again + fgets(line, sizeof(line), fp); + + return ctx->csv_row_count > 0 ? 0 : -EINVAL; +} + +int sim_csv_load(struct sim_csv_context *ctx, + const struct sim_csv_config *config) +{ + if (!ctx || !config) { + return -EINVAL; + } + + memset(ctx, 0, sizeof(*ctx)); + ctx->config = config; + + LOG_INF("═══════════════════════════════════════════════"); + LOG_INF(" CSV DATA LOADING - %s", config->sensor_name); + LOG_INF("═══════════════════════════════════════════════"); + LOG_INF("Attempting to open: %s", config->filename); + + ctx->fp = fopen(config->filename, "r"); + if (!ctx->fp) { + LOG_ERR("Failed to open CSV file: %s", config->filename); + LOG_WRN("Falling back to SYNTHETIC data mode"); + return -ENOENT; + } + + LOG_INF("File opened successfully"); + + // Scan file for metadata + LOG_INF("Scanning file for metadata..."); + if (scan_csv_metadata(ctx->fp, ctx) != 0) { + fclose(ctx->fp); + ctx->fp = NULL; + LOG_ERR("No valid data rows in CSV"); + LOG_WRN("Falling back to SYNTHETIC data mode"); + return -EINVAL; + } + + // Read first two rows into the sliding window + if (read_next_row(ctx->fp, &ctx->row_curr) != 0) { + fclose(ctx->fp); + ctx->fp = NULL; + LOG_ERR("Failed to read first row"); + return -EINVAL; + } + + if (read_next_row(ctx->fp, &ctx->row_next) != 0) { + // Only one row in file - that's OK + ctx->end_of_file = true; + } + + ctx->csv_loaded = true; + ctx->csv_current_index = 0; + + int64_t duration_ms = ctx->csv_last_timestamp - ctx->csv_first_timestamp; + + LOG_INF("═══════════════════════════════════════════════"); + LOG_INF(" CSV LOAD SUCCESSFUL - %s", config->sensor_name); + LOG_INF("═══════════════════════════════════════════════"); + LOG_INF("Rows in file: %zu", ctx->csv_row_count); + LOG_INF("Time range: %lld to %lld ms", + ctx->csv_first_timestamp, ctx->csv_last_timestamp); + LOG_INF("Duration: %.2f seconds", (double)duration_ms / 1000.0); + LOG_INF("Memory usage: %zu bytes (2-row window)", + sizeof(struct sim_csv_row) * 2); + + // Optional: Sensor-specific summary + if (config->log_summary) { + // Need to get last row for summary + struct sim_csv_row temp_first = ctx->row_curr; + struct sim_csv_row temp_last; + + long saved_pos = ftell(ctx->fp); + rewind(ctx->fp); + char line[MAX_LINE_LENGTH]; + fgets(line, sizeof(line), ctx->fp); // skip header + + while (fgets(line, sizeof(line), ctx->fp)) { + parse_csv_row(line, &temp_last); + } + + config->log_summary(&temp_first, &temp_last, ctx->csv_row_count); + + // Restore file position + fseek(ctx->fp, saved_pos, SEEK_SET); + } + + LOG_INF("Mode: CSV PLAYBACK MODE (STREAMING)"); + LOG_INF("═══════════════════════════════════════════════"); + + return 0; +} + +void sim_csv_init_playback(struct sim_csv_context *ctx, + void *sensor_data, + int64_t now_ms) +{ + if (!ctx->csv_loaded) { + return; + } + + ctx->csv_start_time_ms = now_ms; + ctx->sample_count = 0; + + // Initialize with first CSV values + ctx->config->copy_to_sensor(sensor_data, &ctx->row_curr); +} + +void sim_csv_update(struct sim_csv_context *ctx, + void *sensor_data, + int64_t now_ms) +{ + if (!ctx || !ctx->csv_loaded) { + return; + } + + ctx->sample_count++; + + // Calculate elapsed time since start + int64_t elapsed_ms = now_ms - ctx->csv_start_time_ms; + int64_t target_timestamp = ctx->csv_first_timestamp + elapsed_ms; + + // Get timestamps of current window + int64_t curr_ts = sim_csv_get_timestamp_ms(&ctx->row_curr); + int64_t next_ts = ctx->end_of_file ? + ctx->csv_last_timestamp : + sim_csv_get_timestamp_ms(&ctx->row_next); + + // Advance window if needed + while (!ctx->end_of_file && next_ts < target_timestamp) { + // Slide the window + ctx->row_curr = ctx->row_next; + ctx->csv_current_index++; + + // Read new next row + if (read_next_row(ctx->fp, &ctx->row_next) != 0) { + ctx->end_of_file = true; + LOG_DBG("%s: Reached end of CSV file at index %zu", + ctx->config->sensor_name, ctx->csv_current_index); + break; + } + + curr_ts = sim_csv_get_timestamp_ms(&ctx->row_curr); + next_ts = sim_csv_get_timestamp_ms(&ctx->row_next); + + LOG_DBG("%s: CSV window advanced to index %zu (CSV time: %lld ms)", + ctx->config->sensor_name, ctx->csv_current_index, curr_ts); + } + + // Handle edge case: before first data point + if (target_timestamp < curr_ts) { + ctx->config->copy_to_sensor(sensor_data, &ctx->row_curr); + + if (ctx->sample_count % 100 == 0) { + LOG_DBG("%s: Using first CSV row (before start time)", + ctx->config->sensor_name); + } + return; + } + + // Handle edge case: past end of data + if (ctx->end_of_file && target_timestamp >= next_ts) { + ctx->config->copy_to_sensor(sensor_data, &ctx->row_curr); + + if (ctx->sample_count == 1 || (ctx->sample_count % 100 == 0)) { + LOG_WRN("%s: End of CSV data reached - holding last values", + ctx->config->sensor_name); + } + return; + } + + // Interpolate between current and next + int64_t dt = next_ts - curr_ts; + if (dt <= 0) dt = 1; + + float alpha = (float)(target_timestamp - curr_ts) / (float)dt; + if (alpha < 0.0f) alpha = 0.0f; + if (alpha > 1.0f) alpha = 1.0f; + + // Call sensor-specific interpolation + ctx->config->interpolate(sensor_data, &ctx->row_curr, &ctx->row_next, alpha); + + // Periodic logging + if (ctx->sample_count % 50 == 0) { + LOG_INF("%s CSV: idx=%zu/%zu | t=%lld ms | α=%.3f", + ctx->config->sensor_name, + ctx->csv_current_index, ctx->csv_row_count - 1, + target_timestamp, + (double)alpha); + } +} + +void sim_csv_free(struct sim_csv_context *ctx) +{ + if (!ctx) { + return; + } + + if (ctx->fp) { + fclose(ctx->fp); + ctx->fp = NULL; + } + + ctx->csv_loaded = false; + ctx->csv_row_count = 0; +} \ No newline at end of file diff --git a/drivers/sensor/sim_common/sim_csv.h b/drivers/sensor/sim_common/sim_csv.h new file mode 100644 index 0000000..8ef71cd --- /dev/null +++ b/drivers/sensor/sim_common/sim_csv.h @@ -0,0 +1,145 @@ +#pragma once + +#include +#include +#include +#include + + +#ifndef DATA_FILE +#define DATA_FILE "" +#endif + +#define CSV_NUM_COLUMNS 54 + +// Standardized OpenRocket CSV column indices +#define CSV_COL_TIMESTAMP 0 +#define CSV_COL_ALTITUDE 1 +#define CSV_COL_VERTICAL_VELO 2 +#define CSV_COL_VERTICAL_ACCEL 3 +#define CSV_COL_TOTAL_VELO 4 +#define CSV_COL_TOTAL_ACCEL 5 +#define CSV_COL_POS_EAST 6 +#define CSV_COL_POS_NORTH 7 +#define CSV_COL_GPS_LAT_DIST 8 +#define CSV_COL_GPS_LAT_DIR 9 +#define CSV_COL_GPS_LAT_VELO 10 +#define CSV_COL_GPS_LAT_ACCEL 11 +#define CSV_COL_LATITUDE 12 +#define CSV_COL_LONGITUDE 13 +#define CSV_COL_GRAVITY 14 +#define CSV_COL_ANGLE_ATTACK 15 +#define CSV_COL_ROLL_RATE 16 +#define CSV_COL_PITCH_RATE 17 +#define CSV_COL_YAW_RATE 18 +#define CSV_COL_MASS 19 +#define CSV_COL_MOT_MASS 20 +#define CSV_COL_LONG_MMOI 21 +#define CSV_COL_ROT_MMOI 22 +#define CSV_COL_CP_LOCATION 23 +#define CSV_COL_CG_LOCATION 24 +#define CSV_COL_STABILITY 25 +#define CSV_COL_MACH_NUMBER 26 +#define CSV_COL_REYNOLDS_NUMBER 27 +#define CSV_COL_THRUST 28 +#define CSV_COL_DRAG 29 +#define CSV_COL_DRAG_COEFF 30 +#define CSV_COL_AXIAL_DRAG_COEFF 31 +#define CSV_COL_FRIC_DRAG_COEFF 32 +#define CSV_COL_PRESSURE_DRAG_COEFF 33 +#define CSV_COL_BASE_DRAG_COEFF 34 +#define CSV_COL_NORM_FORCE_COEFF 35 +#define CSV_COL_PITCH_MOM_COEFF 36 +#define CSV_COL_YAW_MOM_COEFF 37 +#define CSV_COL_SIDE_FORCE_COEFF 38 +#define CSV_COL_ROLL_MOM_COEFF 39 +#define CSV_COL_ROLL_FORCING_COEFF 40 +#define CSV_COL_ROLL_DAMPING_COEFF 41 +#define CSV_COL_PITCH_DAMPING_COEFF 42 +#define CSV_COL_CORIOLIS_ACCEL 43 +#define CSV_COL_REF_LENGTH 44 +#define CSV_COL_REF_AREA 45 +#define CSV_COL_VERTICAL_ORIENT 46 +#define CSV_COL_LATERAL_ORIENT 47 +#define CSV_COL_WIND_SPEED 48 +#define CSV_COL_AIR_TEMP 49 +#define CSV_COL_AIR_PRESSURE 50 +#define CSV_COL_SPEED_OF_SOUND 51 +#define CSV_COL_SIM_TIMESTEP 52 +#define CSV_COL_COMPUTATION_TIME 53 + +struct sim_csv_row { + float fields[CSV_NUM_COLUMNS]; +}; + +struct sim_csv_config { + const char *filename; + const char *sensor_name; + + /** + * Copy data from CSV row to sensor data structure + * @param sensor_data: Pointer to sensor's data structure + * @param csv_row: Parsed CSV row (array of floats) + */ + void (*copy_to_sensor)(void *sensor_data, const struct sim_csv_row *csv_row); + + /** + * Interpolate between two CSV rows and write to sensor data + * @param sensor_data: Pointer to sensor's data structure + * @param row_curr: Current CSV row + * @param row_next: Next CSV row + * @param alpha: Interpolation factor [0.0, 1.0] + */ + void (*interpolate)(void *sensor_data, const struct sim_csv_row *row_curr, + const struct sim_csv_row *row_next, float alpha); + + /** + * Optional: Log first data point details + */ + void (*log_first_row)(const struct sim_csv_row *row); + + /** + * Optional: Log data range/summary after scanning + */ + void (*log_summary)(const struct sim_csv_row *first_row, + const struct sim_csv_row *last_row, + size_t row_count); +}; + +struct sim_csv_context { + FILE *fp; + + struct sim_csv_row row_curr; // Current CSV row + struct sim_csv_row row_next; // Next CSV row + + int64_t csv_start_time_ms; // Simulation start time + int64_t csv_first_timestamp; // First timestamp in CSV (ms) + int64_t csv_last_timestamp; // Last timestamp in CSV (ms) + + size_t csv_row_count; // Total rows in file + size_t csv_current_index; // Current position + + bool csv_loaded; + bool end_of_file; + + const struct sim_csv_config *config; + uint32_t sample_count; +}; + +int sim_csv_load(struct sim_csv_context *ctx, + const struct sim_csv_config *config); +void sim_csv_update(struct sim_csv_context *ctx, + void *sensor_data, + int64_t now_ms); +void sim_csv_free(struct sim_csv_context *ctx); +void sim_csv_init_playback(struct sim_csv_context *ctx, + void *sensor_data, + int64_t now_ms); + +/** + * Helper: Get timestamp from CSV row (column 0, converted to ms) + */ +static inline int64_t sim_csv_get_timestamp_ms(const struct sim_csv_row *row) +{ + return (int64_t)(row->fields[CSV_COL_TIMESTAMP] * 1000.0f); +} \ No newline at end of file From 161c566402bc324db9cb02c519e507958c0f5c3f Mon Sep 17 00:00:00 2001 From: Jason Date: Wed, 28 Jan 2026 20:07:46 -0800 Subject: [PATCH 7/7] Initial CSV refactoring --- apps/rockets/cloudburst/prj.conf | 5 + drivers/sensor/CMakeLists.txt | 5 +- drivers/sensor/Kconfig | 2 + drivers/sensor/sim_baro/CMakeLists.txt | 5 +- drivers/sensor/sim_baro/sim_baro.c | 396 ++++------------------- drivers/sensor/sim_common/CMakeLists.txt | 3 + drivers/sensor/sim_common/Kconfig | 5 + drivers/sensor/sim_common/sim_csv.c | 306 ++++++++++++++++++ drivers/sensor/sim_common/sim_csv.h | 145 +++++++++ 9 files changed, 528 insertions(+), 344 deletions(-) create mode 100644 drivers/sensor/sim_common/CMakeLists.txt create mode 100644 drivers/sensor/sim_common/Kconfig create mode 100644 drivers/sensor/sim_common/sim_csv.c create mode 100644 drivers/sensor/sim_common/sim_csv.h diff --git a/apps/rockets/cloudburst/prj.conf b/apps/rockets/cloudburst/prj.conf index 7b050b1..e138450 100644 --- a/apps/rockets/cloudburst/prj.conf +++ b/apps/rockets/cloudburst/prj.conf @@ -1,11 +1,16 @@ # Enable simulated sensors for native_sim +CONFIG_SIM_SENSORS=y + +# Enable individual simulated sensors CONFIG_SIM_BARO=y CONFIG_SIM_ACCEL=y CONFIG_SIM_GYRO=y CONFIG_SIM_SDMMC=y +CONFIG_SIM_PYRO=y # Enable fake sensors for native_sim CONFIG_EMUL=y +CONFIG_SPI_EMUL=n CONFIG_SENSOR_SHELL=y # Enable the random number generator for native_sim diff --git a/drivers/sensor/CMakeLists.txt b/drivers/sensor/CMakeLists.txt index 2763f00..edd324a 100644 --- a/drivers/sensor/CMakeLists.txt +++ b/drivers/sensor/CMakeLists.txt @@ -1,5 +1,8 @@ add_subdirectory_ifdef(CONFIG_MS5611 ms5611) + +add_subdirectory_ifdef(CONFIG_SIM_SENSORS sim_common) add_subdirectory_ifdef(CONFIG_SIM_BARO sim_baro) add_subdirectory_ifdef(CONFIG_SIM_ACCEL sim_accel) add_subdirectory_ifdef(CONFIG_SIM_GYRO sim_gyro) -add_subdirectory_ifdef(CONFIG_SIM_SDMMC sim_sdmmc) \ No newline at end of file +add_subdirectory_ifdef(CONFIG_SIM_SDMMC sim_sdmmc) +add_subdirectory_ifdef(CONFIG_SIM_PYRO sim_pyro) \ No newline at end of file diff --git a/drivers/sensor/Kconfig b/drivers/sensor/Kconfig index 6be37f3..a7611b5 100644 --- a/drivers/sensor/Kconfig +++ b/drivers/sensor/Kconfig @@ -1,7 +1,9 @@ if SENSOR rsource "ms5611/Kconfig" +rsource "sim_common/Kconfig" rsource "sim_baro/Kconfig" rsource "sim_accel/Kconfig" rsource "sim_gyro/Kconfig" rsource "sim_sdmmc/Kconfig" +rsource "sim_pyro/Kconfig" endif # SENSOR \ No newline at end of file diff --git a/drivers/sensor/sim_baro/CMakeLists.txt b/drivers/sensor/sim_baro/CMakeLists.txt index d359075..14490fa 100644 --- a/drivers/sensor/sim_baro/CMakeLists.txt +++ b/drivers/sensor/sim_baro/CMakeLists.txt @@ -1,2 +1,5 @@ zephyr_library() -zephyr_library_sources_ifdef(CONFIG_SIM_BARO sim_baro.c) \ No newline at end of file +zephyr_library_sources_ifdef(CONFIG_SIM_BARO sim_baro.c) + +zephyr_include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../sim_common) +zephyr_library_sources(${CMAKE_CURRENT_SOURCE_DIR}/../sim_common/sim_csv.c) \ No newline at end of file diff --git a/drivers/sensor/sim_baro/sim_baro.c b/drivers/sensor/sim_baro/sim_baro.c index 93f5490..a0902a3 100644 --- a/drivers/sensor/sim_baro/sim_baro.c +++ b/drivers/sensor/sim_baro/sim_baro.c @@ -1,85 +1,18 @@ #define DT_DRV_COMPAT zephyr_sim_baro +#include "sim_csv.h" #include #include #include #include #include #include -#include -#include -#include LOG_MODULE_REGISTER(sim_baro, LOG_LEVEL_INF); #ifndef DATA_FILE #define DATA_FILE "" #endif -#define MAX_LINE_LENGTH 512 -#define MAX_CSV_ROWS 10000 - -// OpenRocket CSV Column definitions -#define CSV_COL_TIMESTAMP 0 -#define CSV_COL_ALTITUDE 1 -#define CSV_COL_VERTICAL_VELO 2 -#define CSV_COL_VERTICAL_ACCEL 3 -#define CSV_COL_TOTAL_VELO 4 -#define CSV_COL_TOTAL_ACCEL 5 -#define CSV_COL_POS_EAST 6 -#define CSV_COL_POS_NORTH 7 -#define CSV_COL_GPS_LAT_DIST 8 -#define CSV_COL_GPS_LAT_DIR 9 -#define CSV_COL_GPS_LAT_VELO 10 -#define CSV_COL_GPS_LAT_ACCEL 11 -#define CSV_COL_LATITUDE 12 -#define CSV_COL_LONGITUDE 13 -#define CSV_COL_GRAVITY 14 -#define CSV_COL_ANGLE_ATTACK 15 -#define CSV_COL_ROLL_RATE 16 -#define CSV_COL_PITCH_RATE 17 -#define CSV_COL_YAW_RATE 18 -#define CSV_COL_MASS 19 -#define CSV_COL_MOTOR_MASS 20 -#define CSV_COL_LONG_MMOI 21 -#define CSV_COL_ROT_MMOI 22 -#define CSV_COL_CP_LOCATION 23 -#define CSV_COL_CG_LOCATION 24 -#define CSV_COL_STABILITY 25 -#define CSV_COL_MACH_NUMBER 26 -#define CSV_COL_REYNOLDS_NUMBER 27 -#define CSV_COL_THRUST 28 -#define CSV_COL_DRAG 29 -#define CSV_COL_DRAG_COEFF 30 -#define CSV_COL_AXIAL_DRAG_COEFF 31 -#define CSV_COL_FRIC_DRAG_COEFF 32 -#define CSV_COL_PRESSURE_DRAG_COEFF 33 -#define CSV_COL_BASE_DRAG_COEFF 34 -#define CSV_COL_NORM_FORCE_COEFF 35 -#define CSV_COL_PITCH_MOM_COEFF 36 -#define CSV_COL_YAW_MOM_COEFF 37 -#define CSV_COL_SIDE_FORCE_COEFF 38 -#define CSV_COL_ROLL_MOM_COEFF 39 -#define CSV_COL_ROLL_FORCING_COEFF 40 -#define CSV_COL_ROLL_DAMPING_COEFF 41 -#define CSV_COL_PITCH_DAMPING_COEFF 42 -#define CSV_COL_CORIOLIS_ACCEL 43 -#define CSV_COL_REF_LENGTH 44 -#define CSV_COL_REF_AREA 45 -#define CSV_COL_VERTICAL_ORIENT 46 -#define CSV_COL_LATERAL_ORIENT 47 -#define CSV_COL_WIND_SPEED 48 -#define CSV_COL_AIR_TEMP 49 -#define CSV_COL_AIR_PRESSURE 50 -#define CSV_COL_SPEED_OF_SOUND 51 -#define CSV_COL_SIM_TIMESTEP 52 -#define CSV_COL_COMPUTATION_TIME 53 - -struct csv_row { - int64_t timestamp_ms; - float pressure_mbar; // mbar = hPa - float temperature_c; - float altitude_m; -}; struct sim_baro_data { float altitude_m; @@ -88,298 +21,86 @@ struct sim_baro_data { float temperature_c; int64_t last_ms; - // CSV playback data - struct csv_row *csv_data; - size_t csv_row_count; - size_t csv_current_index; - int64_t csv_start_time_ms; - int64_t csv_first_timestamp; - bool csv_loaded; - - // Logging control - uint32_t sample_count; + struct sim_csv_context csv_ctx; }; - -// Helper to get Nth field from CSV line -static const char* get_csv_field(const char *line, int field_index, char *buffer, size_t buf_size) +static void baro_copy_to_sensor(void *sensor_data, const struct sim_csv_row *csv_row) { - const char *start = line; - const char *end; - int current_field = 0; - - // Skip to the desired field - while (current_field < field_index && *start) { - if (*start == ',') { - current_field++; - } - start++; - } + struct sim_baro_data *data = (struct sim_baro_data *)sensor_data; - if (current_field != field_index) { - return NULL; // Field not found - } - - // Find the end of this field - end = start; - while (*end && *end != ',' && *end != '\n' && *end != '\r') { - end++; - } - - // Copy to buffer - size_t len = end - start; - if (len >= buf_size) { - len = buf_size - 1; - } - - strncpy(buffer, start, len); - buffer[len] = '\0'; - - return buffer; + data->pressure_hpa = csv_row->fields[CSV_COL_AIR_PRESSURE]; + data->temperature_c = csv_row->fields[CSV_COL_AIR_TEMP]; + data->altitude_m = csv_row->fields[CSV_COL_ALTITUDE]; } - - -// Helper macro to extract and parse a field -#define GET_CSV_INT64(line, col, dest) do { \ - char _buf[64]; \ - if (!get_csv_field(line, col, _buf, sizeof(_buf))) return -1; \ - dest = atoll(_buf); \ -} while(0) - -// Helper macro to extract and parse a field -#define GET_CSV_FLOAT(line, col, dest) do { \ - char _buf[64]; \ - if (!get_csv_field(line, col, _buf, sizeof(_buf))) return -1; \ - dest = atof(_buf); \ -} while(0) - -static int parse_csv_line(const char *line, struct csv_row *row) +static void baro_interpolate(void *sensor_data, + const struct sim_csv_row *row_curr, + const struct sim_csv_row *row_next, + float alpha) { - float pressure_raw, temp_raw, timestamp_raw, altitude_raw; + struct sim_baro_data *data = (struct sim_baro_data *)sensor_data; - GET_CSV_FLOAT(line, CSV_COL_TIMESTAMP, timestamp_raw); - GET_CSV_FLOAT(line, CSV_COL_ALTITUDE, altitude_raw); - GET_CSV_FLOAT(line, CSV_COL_AIR_PRESSURE, pressure_raw); - GET_CSV_FLOAT(line, CSV_COL_AIR_TEMP, temp_raw); + data->pressure_hpa = row_curr->fields[CSV_COL_AIR_PRESSURE] + + alpha * (row_next->fields[CSV_COL_AIR_PRESSURE] - + row_curr->fields[CSV_COL_AIR_PRESSURE]); - // Apply unit conversions - row->timestamp_ms = timestamp_raw * 1000.0f; // Convert from seconds to milliseconds - row->altitude_m = altitude_raw; - row->pressure_mbar = pressure_raw; // mbar = hPa - row->temperature_c = temp_raw; + data->temperature_c = row_curr->fields[CSV_COL_AIR_TEMP] + + alpha * (row_next->fields[CSV_COL_AIR_TEMP] - + row_curr->fields[CSV_COL_AIR_TEMP]); - return 0; + data->altitude_m = row_curr->fields[CSV_COL_ALTITUDE] + + alpha * (row_next->fields[CSV_COL_ALTITUDE] - + row_curr->fields[CSV_COL_ALTITUDE]); } - - -static int load_csv_data(struct sim_baro_data *data) +static void baro_log_first_row(const struct sim_csv_row *row) { - LOG_INF("═══════════════════════════════════════════════"); - LOG_INF(" CSV DATA LOADING"); - LOG_INF("═══════════════════════════════════════════════"); - LOG_INF("Attempting to open: %s", DATA_FILE); - - // In Zephyr native_sim, we can use standard C file I/O - FILE *fp = fopen(DATA_FILE, "r"); - if (!fp) { - LOG_ERR("Failed to open CSV file: %s", DATA_FILE); - LOG_WRN("Falling back to SYNTHETIC data mode"); - return -1; - } - - LOG_INF("File opened successfully"); - - // Allocate memory for CSV data - data->csv_data = malloc(sizeof(struct csv_row) * MAX_CSV_ROWS); - if (!data->csv_data) { - fclose(fp); - LOG_ERR("Failed to allocate memory for CSV data"); - LOG_WRN("Falling back to SYNTHETIC data mode"); - return -ENOMEM; - } - - LOG_INF("Memory allocated for %d rows", MAX_CSV_ROWS); - - char line[MAX_LINE_LENGTH]; - data->csv_row_count = 0; - - // Skip header line - if (!fgets(line, sizeof(line), fp)) { - fclose(fp); - k_free(data->csv_data); - LOG_ERR("Empty CSV file"); - LOG_WRN("Falling back to SYNTHETIC data mode"); - return -1; - } - - LOG_INF("Header line read"); - - // Read data rows - while (data->csv_row_count < MAX_CSV_ROWS && fgets(line, sizeof(line), fp)) { - struct csv_row *row = &data->csv_data[data->csv_row_count]; - - if (parse_csv_line(line, row) == 0) { - // Store the first timestamp for relative time calculation - if (data->csv_row_count == 0) { - data->csv_first_timestamp = row->timestamp_ms; - LOG_INF("First data point: t=%lld ms, p=%.2f mbar, alt=%.2f m, T=%.2f C", - row->timestamp_ms, - (double)row->pressure_mbar, - (double)row->altitude_m, - (double)row->temperature_c); - } - data->csv_row_count++; - } - } - - fclose(fp); - - if (data->csv_row_count == 0) { - k_free(data->csv_data); - LOG_ERR("No valid data rows in CSV"); - LOG_WRN("Falling back to SYNTHETIC data mode"); - return -1; - } - - data->csv_loaded = true; - - struct csv_row *last_row = &data->csv_data[data->csv_row_count - 1]; - int64_t duration_ms = last_row->timestamp_ms - data->csv_first_timestamp; - - LOG_INF("═══════════════════════════════════════════════"); - LOG_INF(" CSV LOAD SUCCESSFUL"); - LOG_INF("═══════════════════════════════════════════════"); - LOG_INF("Rows loaded: %zu", data->csv_row_count); - LOG_INF("Time range: %lld to %lld ms", - data->csv_data[0].timestamp_ms, - last_row->timestamp_ms); - LOG_INF("Duration: %.2f seconds", (double)duration_ms / 1000.0); - LOG_INF("Altitude range: %.2f to %.2f m", - (double)data->csv_data[0].altitude_m, - (double)last_row->altitude_m); - LOG_INF("Mode: CSV PLAYBACK MODE"); - LOG_INF("═══════════════════════════════════════════════"); - - return 0; + LOG_INF("First data point: t=%.3f s, p=%.2f hPa, alt=%.2f m, T=%.2f C", + (double)row->fields[CSV_COL_TIMESTAMP], + (double)row->fields[CSV_COL_AIR_PRESSURE], + (double)row->fields[CSV_COL_ALTITUDE], + (double)row->fields[CSV_COL_AIR_TEMP]); } -static void interpolate_csv_data(struct sim_baro_data *data, int64_t now_ms) +static void baro_log_summary(const struct sim_csv_row *first_row, + const struct sim_csv_row *last_row, + size_t row_count) { - if (!data->csv_loaded || data->csv_row_count == 0) { - return; - } - - // Calculate elapsed time since start (in simulation time) - int64_t elapsed_ms = now_ms - data->csv_start_time_ms; - - // Map to CSV timestamp (relative to first CSV timestamp) - int64_t target_timestamp = data->csv_first_timestamp + elapsed_ms; - - // Find the two surrounding data points - size_t i = data->csv_current_index; - - // Advance index if needed - while (i < data->csv_row_count - 1 && - data->csv_data[i + 1].timestamp_ms <= target_timestamp) { - i++; - } - - // Log when we advance to a new index - if (i != data->csv_current_index) { - LOG_DBG("CSV index advanced: %zu -> %zu (CSV time: %lld ms)", - data->csv_current_index, i, - data->csv_data[i].timestamp_ms); - } - - data->csv_current_index = i; - - // Handle edge cases - if (i == 0 && target_timestamp < data->csv_data[0].timestamp_ms) { - // Before first data point, use first row - struct csv_row *row = &data->csv_data[0]; - data->pressure_hpa = row->pressure_mbar; // mbar = hPa - data->temperature_c = row->temperature_c; - data->altitude_m = row->altitude_m; - - if (data->sample_count % 100 == 0) { - LOG_DBG("Using first CSV row (before start time)"); - } - return; - } - - if (i >= data->csv_row_count - 1) { - // Past end of data, use last row - struct csv_row *row = &data->csv_data[data->csv_row_count - 1]; - data->pressure_hpa = row->pressure_mbar; - data->temperature_c = row->temperature_c; - data->altitude_m = row->altitude_m; - - if (data->sample_count == 0 || (data->sample_count % 100 == 0)) { - LOG_WRN("End of CSV data reached - holding last values"); - } - return; - } - - // Interpolate between current and next point - struct csv_row *curr = &data->csv_data[i]; - struct csv_row *next = &data->csv_data[i + 1]; - - int64_t dt = next->timestamp_ms - curr->timestamp_ms; - if (dt <= 0) dt = 1; - - float alpha = (float)(target_timestamp - curr->timestamp_ms) / (float)dt; - if (alpha < 0.0f) alpha = 0.0f; - if (alpha > 1.0f) alpha = 1.0f; - - // Linear interpolation - data->pressure_hpa = curr->pressure_mbar + alpha * (next->pressure_mbar - curr->pressure_mbar); - data->temperature_c = curr->temperature_c + alpha * (next->temperature_c - curr->temperature_c); - data->altitude_m = curr->altitude_m + alpha * (next->altitude_m - curr->altitude_m); - - // Periodic detailed logging (every 50 samples) - if (data->sample_count % 50 == 0) { - LOG_INF("CSV: idx=%zu/%zu | t=%lld ms | p=%.2f hPa | alt=%.2f m | T=%.2f C | α=%.3f", - i, data->csv_row_count - 1, - target_timestamp, - (double)data->pressure_hpa, - (double)data->altitude_m, - (double)data->temperature_c, - (double)alpha); - } + LOG_INF("Altitude range: %.2f to %.2f m", + (double)first_row->fields[CSV_COL_ALTITUDE], + (double)last_row->fields[CSV_COL_ALTITUDE]); } +static const struct sim_csv_config baro_csv_config = { + .filename = DATA_FILE, + .sensor_name = "BARO", + .copy_to_sensor = baro_copy_to_sensor, + .interpolate = baro_interpolate, + .log_first_row = baro_log_first_row, + .log_summary = baro_log_summary, +}; + static int sim_baro_sample_fetch(const struct device *dev, enum sensor_channel chan) { struct sim_baro_data *data = dev->data; - int64_t now = k_uptime_get(); float dt = (now - data->last_ms) / 1000.0f; // Initialize on first call if (data->last_ms == 0) { data->last_ms = now; - data->sample_count = 0; LOG_INF("═══════════════════════════════════════════════"); LOG_INF(" SIM_BARO INITIALIZATION"); LOG_INF("═══════════════════════════════════════════════"); - // Load CSV data if DATA_FILE is defined + // Try to load CSV data if (strlen(DATA_FILE) > 0) { LOG_INF("DATA_FILE defined: \"%s\"", DATA_FILE); - if (load_csv_data(data) == 0) { - data->csv_start_time_ms = now; - data->csv_current_index = 0; - - // Initialize with first CSV values - struct csv_row *first = &data->csv_data[0]; - data->pressure_hpa = first->pressure_mbar; - data->temperature_c = first->temperature_c; - data->altitude_m = first->altitude_m; + if (sim_csv_load(&data->csv_ctx, &baro_csv_config) == 0) { + sim_csv_init_playback(&data->csv_ctx, data, now); } } else { LOG_INF("DATA_FILE not defined (empty string)"); @@ -393,43 +114,36 @@ static int sim_baro_sample_fetch(const struct device *dev, data->temperature_c = 20.0f; } - return 0; // Now returns with valid data + return 0; } data->last_ms = now; - data->sample_count++; - if (dt <= 0) dt = 0.02f; - if (strlen(DATA_FILE) > 0 && data->csv_loaded) { + if (data->csv_ctx.csv_loaded) { // Use CSV data - interpolate_csv_data(data, now); + sim_csv_update(&data->csv_ctx, data, now); } else { - // Synthetic data + // Synthetic data mode data->velocity_mps += 0.1f * dt; - data->altitude_m += data->velocity_mps * dt; + data->altitude_m += data->velocity_mps * dt; - /* Add noise */ float noise = ((int32_t)sys_rand32_get() % 1000) / 1000.0f - 0.5f; data->altitude_m += noise * 0.05f; - /* Convert altitude → pressure (ISA approx), result in hPa */ float pressure_pa = 101325.0f * powf(1.0f - (data->altitude_m / 44330.0f), 5.255f); data->pressure_hpa = pressure_pa / 100.0f; - data->temperature_c = 20.0f; - // Periodic logging for synthetic mode (every 50 samples) - if (data->sample_count % 50 == 0) { + if (data->csv_ctx.sample_count % 50 == 0) { LOG_INF("SYN: p=%.2f hPa | alt=%.2f m | v=%.2f m/s | T=%.2f C", - (double)data->pressure_hpa, - (double)data->altitude_m, - (double)data->velocity_mps, - (double)data->temperature_c); + (double)data->pressure_hpa, (double)data->altitude_m, + (double)data->velocity_mps, (double)data->temperature_c); } + data->csv_ctx.sample_count++; } return 0; @@ -461,8 +175,6 @@ static const struct sensor_driver_api sim_baro_api = { #define SIM_BARO_INIT(inst) \ static struct sim_baro_data sim_baro_data_##inst = { \ .last_ms = 0, \ - .csv_loaded = false, \ - .sample_count = 0, \ .pressure_hpa = 1013.25f, \ .temperature_c = 20.0f, \ .altitude_m = 0.0f, \ diff --git a/drivers/sensor/sim_common/CMakeLists.txt b/drivers/sensor/sim_common/CMakeLists.txt new file mode 100644 index 0000000..ffc4e9b --- /dev/null +++ b/drivers/sensor/sim_common/CMakeLists.txt @@ -0,0 +1,3 @@ +zephyr_library() +zephyr_library_sources(sim_csv.c) +zephyr_include_directories(${CMAKE_CURRENT_SOURCE_DIR}) \ No newline at end of file diff --git a/drivers/sensor/sim_common/Kconfig b/drivers/sensor/sim_common/Kconfig new file mode 100644 index 0000000..f908112 --- /dev/null +++ b/drivers/sensor/sim_common/Kconfig @@ -0,0 +1,5 @@ +config SIM_SENSORS + bool "Simulated sensors for native_sim" + depends on DT_HAS_ZEPHYR_SIM_SENSORS_ENABLED + help + Enable simulated sensors for Zephyr's native sim \ No newline at end of file diff --git a/drivers/sensor/sim_common/sim_csv.c b/drivers/sensor/sim_common/sim_csv.c new file mode 100644 index 0000000..992750a --- /dev/null +++ b/drivers/sensor/sim_common/sim_csv.c @@ -0,0 +1,306 @@ +#include "sim_csv.h" +#include +#include +#include +#include +#include +#include + +LOG_MODULE_REGISTER(sim_csv, LOG_LEVEL_INF); + +#define MAX_LINE_LENGTH 4096 + +/** + * Parse entire CSV line into float array using sscanf + * Returns 0 on success, -1 on error + */ +static int parse_csv_row(const char *line, struct sim_csv_row *row) +{ + int result = sscanf(line, + "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f," // 0-9 + "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f," // 10-19 + "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f," // 20-29 + "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f," // 30-39 + "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f," // 40-49 + "%f,%f,%f,%f", // 50-53 + &row->fields[0], &row->fields[1], &row->fields[2], &row->fields[3], + &row->fields[4], &row->fields[5], &row->fields[6], &row->fields[7], + &row->fields[8], &row->fields[9], &row->fields[10], &row->fields[11], + &row->fields[12], &row->fields[13], &row->fields[14], &row->fields[15], + &row->fields[16], &row->fields[17], &row->fields[18], &row->fields[19], + &row->fields[20], &row->fields[21], &row->fields[22], &row->fields[23], + &row->fields[24], &row->fields[25], &row->fields[26], &row->fields[27], + &row->fields[28], &row->fields[29], &row->fields[30], &row->fields[31], + &row->fields[32], &row->fields[33], &row->fields[34], &row->fields[35], + &row->fields[36], &row->fields[37], &row->fields[38], &row->fields[39], + &row->fields[40], &row->fields[41], &row->fields[42], &row->fields[43], + &row->fields[44], &row->fields[45], &row->fields[46], &row->fields[47], + &row->fields[48], &row->fields[49], &row->fields[50], &row->fields[51], + &row->fields[52], &row->fields[53] + ); + + return (result == CSV_NUM_COLUMNS) ? 0 : -1; +} + +/** + * Read next CSV row from file + * Returns 0 on success, -1 on EOF or error + */ +static int read_next_row(FILE *fp, struct sim_csv_row *row_out) +{ + char line[MAX_LINE_LENGTH]; + + if (!fgets(line, sizeof(line), fp)) { + return -1; // EOF or error + } + + return parse_csv_row(line, row_out); +} + +/** + * Scan entire file to get metadata + */ +static int scan_csv_metadata(FILE *fp, struct sim_csv_context *ctx) +{ + char line[MAX_LINE_LENGTH]; + struct sim_csv_row temp_row; + + ctx->csv_row_count = 0; + bool first_row = true; + + // Skip header + if (!fgets(line, sizeof(line), fp)) { + return -EINVAL; + } + + // Scan all data rows + while (fgets(line, sizeof(line), fp)) { + if (parse_csv_row(line, &temp_row) == 0) { + int64_t timestamp = sim_csv_get_timestamp_ms(&temp_row); + + if (first_row) { + ctx->csv_first_timestamp = timestamp; + + if (ctx->config->log_first_row) { + ctx->config->log_first_row(&temp_row); + } + + first_row = false; + } + + ctx->csv_last_timestamp = timestamp; + ctx->csv_row_count++; + } + } + + // Rewind to start of file + rewind(fp); + + // Skip header again + fgets(line, sizeof(line), fp); + + return ctx->csv_row_count > 0 ? 0 : -EINVAL; +} + +int sim_csv_load(struct sim_csv_context *ctx, + const struct sim_csv_config *config) +{ + if (!ctx || !config) { + return -EINVAL; + } + + memset(ctx, 0, sizeof(*ctx)); + ctx->config = config; + + LOG_INF("═══════════════════════════════════════════════"); + LOG_INF(" CSV DATA LOADING - %s", config->sensor_name); + LOG_INF("═══════════════════════════════════════════════"); + LOG_INF("Attempting to open: %s", config->filename); + + ctx->fp = fopen(config->filename, "r"); + if (!ctx->fp) { + LOG_ERR("Failed to open CSV file: %s", config->filename); + LOG_WRN("Falling back to SYNTHETIC data mode"); + return -ENOENT; + } + + LOG_INF("File opened successfully"); + + // Scan file for metadata + LOG_INF("Scanning file for metadata..."); + if (scan_csv_metadata(ctx->fp, ctx) != 0) { + fclose(ctx->fp); + ctx->fp = NULL; + LOG_ERR("No valid data rows in CSV"); + LOG_WRN("Falling back to SYNTHETIC data mode"); + return -EINVAL; + } + + // Read first two rows into the sliding window + if (read_next_row(ctx->fp, &ctx->row_curr) != 0) { + fclose(ctx->fp); + ctx->fp = NULL; + LOG_ERR("Failed to read first row"); + return -EINVAL; + } + + if (read_next_row(ctx->fp, &ctx->row_next) != 0) { + // Only one row in file - that's OK + ctx->end_of_file = true; + } + + ctx->csv_loaded = true; + ctx->csv_current_index = 0; + + int64_t duration_ms = ctx->csv_last_timestamp - ctx->csv_first_timestamp; + + LOG_INF("═══════════════════════════════════════════════"); + LOG_INF(" CSV LOAD SUCCESSFUL - %s", config->sensor_name); + LOG_INF("═══════════════════════════════════════════════"); + LOG_INF("Rows in file: %zu", ctx->csv_row_count); + LOG_INF("Time range: %lld to %lld ms", + ctx->csv_first_timestamp, ctx->csv_last_timestamp); + LOG_INF("Duration: %.2f seconds", (double)duration_ms / 1000.0); + LOG_INF("Memory usage: %zu bytes (2-row window)", + sizeof(struct sim_csv_row) * 2); + + // Optional: Sensor-specific summary + if (config->log_summary) { + // Need to get last row for summary + struct sim_csv_row temp_first = ctx->row_curr; + struct sim_csv_row temp_last; + + long saved_pos = ftell(ctx->fp); + rewind(ctx->fp); + char line[MAX_LINE_LENGTH]; + fgets(line, sizeof(line), ctx->fp); // skip header + + while (fgets(line, sizeof(line), ctx->fp)) { + parse_csv_row(line, &temp_last); + } + + config->log_summary(&temp_first, &temp_last, ctx->csv_row_count); + + // Restore file position + fseek(ctx->fp, saved_pos, SEEK_SET); + } + + LOG_INF("Mode: CSV PLAYBACK MODE (STREAMING)"); + LOG_INF("═══════════════════════════════════════════════"); + + return 0; +} + +void sim_csv_init_playback(struct sim_csv_context *ctx, + void *sensor_data, + int64_t now_ms) +{ + if (!ctx->csv_loaded) { + return; + } + + ctx->csv_start_time_ms = now_ms; + ctx->sample_count = 0; + + // Initialize with first CSV values + ctx->config->copy_to_sensor(sensor_data, &ctx->row_curr); +} + +void sim_csv_update(struct sim_csv_context *ctx, + void *sensor_data, + int64_t now_ms) +{ + if (!ctx || !ctx->csv_loaded) { + return; + } + + ctx->sample_count++; + + // Calculate elapsed time since start + int64_t elapsed_ms = now_ms - ctx->csv_start_time_ms; + int64_t target_timestamp = ctx->csv_first_timestamp + elapsed_ms; + + // Get timestamps of current window + int64_t curr_ts = sim_csv_get_timestamp_ms(&ctx->row_curr); + int64_t next_ts = ctx->end_of_file ? + ctx->csv_last_timestamp : + sim_csv_get_timestamp_ms(&ctx->row_next); + + // Advance window if needed + while (!ctx->end_of_file && next_ts < target_timestamp) { + // Slide the window + ctx->row_curr = ctx->row_next; + ctx->csv_current_index++; + + // Read new next row + if (read_next_row(ctx->fp, &ctx->row_next) != 0) { + ctx->end_of_file = true; + LOG_DBG("%s: Reached end of CSV file at index %zu", + ctx->config->sensor_name, ctx->csv_current_index); + break; + } + + curr_ts = sim_csv_get_timestamp_ms(&ctx->row_curr); + next_ts = sim_csv_get_timestamp_ms(&ctx->row_next); + + LOG_DBG("%s: CSV window advanced to index %zu (CSV time: %lld ms)", + ctx->config->sensor_name, ctx->csv_current_index, curr_ts); + } + + // Handle edge case: before first data point + if (target_timestamp < curr_ts) { + ctx->config->copy_to_sensor(sensor_data, &ctx->row_curr); + + if (ctx->sample_count % 100 == 0) { + LOG_DBG("%s: Using first CSV row (before start time)", + ctx->config->sensor_name); + } + return; + } + + // Handle edge case: past end of data + if (ctx->end_of_file && target_timestamp >= next_ts) { + ctx->config->copy_to_sensor(sensor_data, &ctx->row_curr); + + if (ctx->sample_count == 1 || (ctx->sample_count % 100 == 0)) { + LOG_WRN("%s: End of CSV data reached - holding last values", + ctx->config->sensor_name); + } + return; + } + + // Interpolate between current and next + int64_t dt = next_ts - curr_ts; + if (dt <= 0) dt = 1; + + float alpha = (float)(target_timestamp - curr_ts) / (float)dt; + if (alpha < 0.0f) alpha = 0.0f; + if (alpha > 1.0f) alpha = 1.0f; + + // Call sensor-specific interpolation + ctx->config->interpolate(sensor_data, &ctx->row_curr, &ctx->row_next, alpha); + + // Periodic logging + if (ctx->sample_count % 50 == 0) { + LOG_INF("%s CSV: idx=%zu/%zu | t=%lld ms | α=%.3f", + ctx->config->sensor_name, + ctx->csv_current_index, ctx->csv_row_count - 1, + target_timestamp, + (double)alpha); + } +} + +void sim_csv_free(struct sim_csv_context *ctx) +{ + if (!ctx) { + return; + } + + if (ctx->fp) { + fclose(ctx->fp); + ctx->fp = NULL; + } + + ctx->csv_loaded = false; + ctx->csv_row_count = 0; +} \ No newline at end of file diff --git a/drivers/sensor/sim_common/sim_csv.h b/drivers/sensor/sim_common/sim_csv.h new file mode 100644 index 0000000..8ef71cd --- /dev/null +++ b/drivers/sensor/sim_common/sim_csv.h @@ -0,0 +1,145 @@ +#pragma once + +#include +#include +#include +#include + + +#ifndef DATA_FILE +#define DATA_FILE "" +#endif + +#define CSV_NUM_COLUMNS 54 + +// Standardized OpenRocket CSV column indices +#define CSV_COL_TIMESTAMP 0 +#define CSV_COL_ALTITUDE 1 +#define CSV_COL_VERTICAL_VELO 2 +#define CSV_COL_VERTICAL_ACCEL 3 +#define CSV_COL_TOTAL_VELO 4 +#define CSV_COL_TOTAL_ACCEL 5 +#define CSV_COL_POS_EAST 6 +#define CSV_COL_POS_NORTH 7 +#define CSV_COL_GPS_LAT_DIST 8 +#define CSV_COL_GPS_LAT_DIR 9 +#define CSV_COL_GPS_LAT_VELO 10 +#define CSV_COL_GPS_LAT_ACCEL 11 +#define CSV_COL_LATITUDE 12 +#define CSV_COL_LONGITUDE 13 +#define CSV_COL_GRAVITY 14 +#define CSV_COL_ANGLE_ATTACK 15 +#define CSV_COL_ROLL_RATE 16 +#define CSV_COL_PITCH_RATE 17 +#define CSV_COL_YAW_RATE 18 +#define CSV_COL_MASS 19 +#define CSV_COL_MOT_MASS 20 +#define CSV_COL_LONG_MMOI 21 +#define CSV_COL_ROT_MMOI 22 +#define CSV_COL_CP_LOCATION 23 +#define CSV_COL_CG_LOCATION 24 +#define CSV_COL_STABILITY 25 +#define CSV_COL_MACH_NUMBER 26 +#define CSV_COL_REYNOLDS_NUMBER 27 +#define CSV_COL_THRUST 28 +#define CSV_COL_DRAG 29 +#define CSV_COL_DRAG_COEFF 30 +#define CSV_COL_AXIAL_DRAG_COEFF 31 +#define CSV_COL_FRIC_DRAG_COEFF 32 +#define CSV_COL_PRESSURE_DRAG_COEFF 33 +#define CSV_COL_BASE_DRAG_COEFF 34 +#define CSV_COL_NORM_FORCE_COEFF 35 +#define CSV_COL_PITCH_MOM_COEFF 36 +#define CSV_COL_YAW_MOM_COEFF 37 +#define CSV_COL_SIDE_FORCE_COEFF 38 +#define CSV_COL_ROLL_MOM_COEFF 39 +#define CSV_COL_ROLL_FORCING_COEFF 40 +#define CSV_COL_ROLL_DAMPING_COEFF 41 +#define CSV_COL_PITCH_DAMPING_COEFF 42 +#define CSV_COL_CORIOLIS_ACCEL 43 +#define CSV_COL_REF_LENGTH 44 +#define CSV_COL_REF_AREA 45 +#define CSV_COL_VERTICAL_ORIENT 46 +#define CSV_COL_LATERAL_ORIENT 47 +#define CSV_COL_WIND_SPEED 48 +#define CSV_COL_AIR_TEMP 49 +#define CSV_COL_AIR_PRESSURE 50 +#define CSV_COL_SPEED_OF_SOUND 51 +#define CSV_COL_SIM_TIMESTEP 52 +#define CSV_COL_COMPUTATION_TIME 53 + +struct sim_csv_row { + float fields[CSV_NUM_COLUMNS]; +}; + +struct sim_csv_config { + const char *filename; + const char *sensor_name; + + /** + * Copy data from CSV row to sensor data structure + * @param sensor_data: Pointer to sensor's data structure + * @param csv_row: Parsed CSV row (array of floats) + */ + void (*copy_to_sensor)(void *sensor_data, const struct sim_csv_row *csv_row); + + /** + * Interpolate between two CSV rows and write to sensor data + * @param sensor_data: Pointer to sensor's data structure + * @param row_curr: Current CSV row + * @param row_next: Next CSV row + * @param alpha: Interpolation factor [0.0, 1.0] + */ + void (*interpolate)(void *sensor_data, const struct sim_csv_row *row_curr, + const struct sim_csv_row *row_next, float alpha); + + /** + * Optional: Log first data point details + */ + void (*log_first_row)(const struct sim_csv_row *row); + + /** + * Optional: Log data range/summary after scanning + */ + void (*log_summary)(const struct sim_csv_row *first_row, + const struct sim_csv_row *last_row, + size_t row_count); +}; + +struct sim_csv_context { + FILE *fp; + + struct sim_csv_row row_curr; // Current CSV row + struct sim_csv_row row_next; // Next CSV row + + int64_t csv_start_time_ms; // Simulation start time + int64_t csv_first_timestamp; // First timestamp in CSV (ms) + int64_t csv_last_timestamp; // Last timestamp in CSV (ms) + + size_t csv_row_count; // Total rows in file + size_t csv_current_index; // Current position + + bool csv_loaded; + bool end_of_file; + + const struct sim_csv_config *config; + uint32_t sample_count; +}; + +int sim_csv_load(struct sim_csv_context *ctx, + const struct sim_csv_config *config); +void sim_csv_update(struct sim_csv_context *ctx, + void *sensor_data, + int64_t now_ms); +void sim_csv_free(struct sim_csv_context *ctx); +void sim_csv_init_playback(struct sim_csv_context *ctx, + void *sensor_data, + int64_t now_ms); + +/** + * Helper: Get timestamp from CSV row (column 0, converted to ms) + */ +static inline int64_t sim_csv_get_timestamp_ms(const struct sim_csv_row *row) +{ + return (int64_t)(row->fields[CSV_COL_TIMESTAMP] * 1000.0f); +} \ No newline at end of file