From 0e277acf878cdc9bc16bcab3d80a8042d2693bbe Mon Sep 17 00:00:00 2001 From: aychar Date: Mon, 15 Sep 2025 18:50:37 -0500 Subject: [PATCH 1/2] add lis2mdl driver and implement in imu component --- .../Components/CMakeLists.txt | 1 + FprimeZephyrReference/Components/Imu/Imu.cpp | 20 +----- FprimeZephyrReference/Components/Imu/Imu.fpp | 15 ++--- FprimeZephyrReference/Components/Imu/Imu.hpp | 6 -- .../Components/Lis2mdlDriver/CMakeLists.txt | 36 ++++++++++ .../Lis2mdlDriver/Lis2mdlDriver.cpp | 44 +++++++++++++ .../Lis2mdlDriver/Lis2mdlDriver.fpp | 24 +++++++ .../Lis2mdlDriver/Lis2mdlDriver.hpp | 52 +++++++++++++++ .../Components/Lis2mdlDriver/docs/sdd.md | 66 +++++++++++++++++++ .../Top/ReferenceDeploymentPackets.fppi | 1 - .../ReferenceDeployment/Top/instances.fpp | 2 + .../ReferenceDeployment/Top/topology.fpp | 5 ++ 12 files changed, 236 insertions(+), 36 deletions(-) create mode 100644 FprimeZephyrReference/Components/Lis2mdlDriver/CMakeLists.txt create mode 100644 FprimeZephyrReference/Components/Lis2mdlDriver/Lis2mdlDriver.cpp create mode 100644 FprimeZephyrReference/Components/Lis2mdlDriver/Lis2mdlDriver.fpp create mode 100644 FprimeZephyrReference/Components/Lis2mdlDriver/Lis2mdlDriver.hpp create mode 100644 FprimeZephyrReference/Components/Lis2mdlDriver/docs/sdd.md diff --git a/FprimeZephyrReference/Components/CMakeLists.txt b/FprimeZephyrReference/Components/CMakeLists.txt index 7b0d52e4..2a88e633 100644 --- a/FprimeZephyrReference/Components/CMakeLists.txt +++ b/FprimeZephyrReference/Components/CMakeLists.txt @@ -3,3 +3,4 @@ add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/FatalHandler") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/Watchdog") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/Imu/") +add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/Lis2mdlDriver/") diff --git a/FprimeZephyrReference/Components/Imu/Imu.cpp b/FprimeZephyrReference/Components/Imu/Imu.cpp index 2af08aa3..5cbcc3d7 100644 --- a/FprimeZephyrReference/Components/Imu/Imu.cpp +++ b/FprimeZephyrReference/Components/Imu/Imu.cpp @@ -14,10 +14,6 @@ namespace Components { // ---------------------------------------------------------------------- Imu ::Imu(const char* const compName) : ImuComponentBase(compName) { - // Initialize the LIS2MDL sensor - lis2mdl = device_get_binding("LIS2MDL"); - FW_ASSERT(device_is_ready(lis2mdl)); - // Initialize the LSM6DSO sensor lsm6dso = DEVICE_DT_GET_ONE(st_lsm6dso); FW_ASSERT(device_is_ready(lsm6dso)); @@ -38,13 +34,12 @@ void Imu ::run_handler(FwIndexType portNum, U32 context) { // Fetch new data samples from the sensors sensor_sample_fetch_chan(lsm6dso, SENSOR_CHAN_ACCEL_XYZ); sensor_sample_fetch_chan(lsm6dso, SENSOR_CHAN_GYRO_XYZ); - sensor_sample_fetch_chan(lis2mdl, SENSOR_CHAN_MAGN_XYZ); sensor_sample_fetch_chan(lsm6dso, SENSOR_CHAN_DIE_TEMP); // Output sensor values via telemetry this->tlmWrite_Acceleration(this->get_acceleration()); this->tlmWrite_AngularVelocity(this->get_angular_velocity()); - this->tlmWrite_MagneticField(this->get_magnetic_field()); + this->tlmWrite_MagneticField(this->readMagneticField_out(0)); this->tlmWrite_Temperature(this->get_temperature()); } @@ -78,19 +73,6 @@ Components::Imu_AngularVelocity Imu ::get_angular_velocity() { this->sensor_value_to_f64(z)); } -Components::Imu_MagneticField Imu ::get_magnetic_field() { - struct sensor_value x; - struct sensor_value y; - struct sensor_value z; - - sensor_channel_get(lis2mdl, SENSOR_CHAN_MAGN_X, &x); - sensor_channel_get(lis2mdl, SENSOR_CHAN_MAGN_Y, &y); - sensor_channel_get(lis2mdl, SENSOR_CHAN_MAGN_Z, &z); - - return Components::Imu_MagneticField(this->sensor_value_to_f64(x), this->sensor_value_to_f64(y), - this->sensor_value_to_f64(z)); -} - F64 Imu ::get_temperature() { struct sensor_value temp; diff --git a/FprimeZephyrReference/Components/Imu/Imu.fpp b/FprimeZephyrReference/Components/Imu/Imu.fpp index 56d12660..30a46fa5 100644 --- a/FprimeZephyrReference/Components/Imu/Imu.fpp +++ b/FprimeZephyrReference/Components/Imu/Imu.fpp @@ -3,6 +3,8 @@ module Components { passive component Imu { sync input port run: Svc.Sched + output port readMagneticField: MagneticFieldRead + @ Acceleration reading in m/s^2 struct Acceleration { x: F64 @@ -13,6 +15,9 @@ module Components { @ Telemetry channel for acceleration telemetry Acceleration: Acceleration + @ Telemetry channel for magnetic field + telemetry MagneticField: MagneticField + @ Angular velocity reading in rad/s struct AngularVelocity { x: F64 @@ -23,16 +28,6 @@ module Components { @ Telemetry channel for angular velocity telemetry AngularVelocity: AngularVelocity - @ Magnetic field reading in gauss - struct MagneticField { - x: F64 - y: F64 - z: F64 - } - - @ Telemetry channel for magnetic field - telemetry MagneticField: MagneticField - @ Telemetry channel for temperature in degrees Celsius telemetry Temperature: F64 diff --git a/FprimeZephyrReference/Components/Imu/Imu.hpp b/FprimeZephyrReference/Components/Imu/Imu.hpp index 2484fdb2..43ca3738 100644 --- a/FprimeZephyrReference/Components/Imu/Imu.hpp +++ b/FprimeZephyrReference/Components/Imu/Imu.hpp @@ -53,9 +53,6 @@ class Imu final : public ImuComponentBase { //! Get the angular velocity reading from the IMU Components::Imu_AngularVelocity get_angular_velocity(); - //! Get the magnetic field reading from the IMU - Components::Imu_MagneticField get_magnetic_field(); - //! Get the temperature reading from the IMU F64 get_temperature(); @@ -63,9 +60,6 @@ class Imu final : public ImuComponentBase { // Member variables // ---------------------------------------------------------------------- - //! Zephyr device stores the initialized LIS2MDL sensor - const struct device* lis2mdl; - //! Zephyr device stores the initialized LSM6DSO sensor const struct device* lsm6dso; }; diff --git a/FprimeZephyrReference/Components/Lis2mdlDriver/CMakeLists.txt b/FprimeZephyrReference/Components/Lis2mdlDriver/CMakeLists.txt new file mode 100644 index 00000000..ed403e6b --- /dev/null +++ b/FprimeZephyrReference/Components/Lis2mdlDriver/CMakeLists.txt @@ -0,0 +1,36 @@ +#### +# F Prime CMakeLists.txt: +# +# SOURCES: list of source files (to be compiled) +# AUTOCODER_INPUTS: list of files to be passed to the autocoders +# DEPENDS: list of libraries that this module depends on +# +# More information in the F´ CMake API documentation: +# https://fprime.jpl.nasa.gov/latest/docs/reference/api/cmake/API/ +# +#### + +# Module names are derived from the path from the nearest project/library/framework +# root when not specifically overridden by the developer. i.e. The module defined by +# `Ref/SignalGen/CMakeLists.txt` will be named `Ref_SignalGen`. + +register_fprime_library( + AUTOCODER_INPUTS + "${CMAKE_CURRENT_LIST_DIR}/Lis2mdlDriver.fpp" + SOURCES + "${CMAKE_CURRENT_LIST_DIR}/Lis2mdlDriver.cpp" +# DEPENDS +# MyPackage_MyOtherModule +) + +### Unit Tests ### +# register_fprime_ut( +# AUTOCODER_INPUTS +# "${CMAKE_CURRENT_LIST_DIR}/Lis2mdlDriver.fpp" +# SOURCES +# "${CMAKE_CURRENT_LIST_DIR}/test/ut/Lis2mdlDriverTestMain.cpp" +# "${CMAKE_CURRENT_LIST_DIR}/test/ut/Lis2mdlDriverTester.cpp" +# DEPENDS +# STest # For rules-based testing +# UT_AUTO_HELPERS +# ) diff --git a/FprimeZephyrReference/Components/Lis2mdlDriver/Lis2mdlDriver.cpp b/FprimeZephyrReference/Components/Lis2mdlDriver/Lis2mdlDriver.cpp new file mode 100644 index 00000000..9dacc1bd --- /dev/null +++ b/FprimeZephyrReference/Components/Lis2mdlDriver/Lis2mdlDriver.cpp @@ -0,0 +1,44 @@ +// ====================================================================== +// \title Lis2mdlDriver.cpp +// \author aychar +// \brief cpp file for Lis2mdlDriver component implementation class +// ====================================================================== + +#include "FprimeZephyrReference/Components/Lis2mdlDriver/Lis2mdlDriver.hpp" + +#include + +namespace Components { + +// ---------------------------------------------------------------------- +// Component construction and destruction +// ---------------------------------------------------------------------- + +Lis2mdlDriver ::Lis2mdlDriver(const char* const compName) : Lis2mdlDriverComponentBase(compName) { + lis2mdl = device_get_binding("LIS2MDL"); + FW_ASSERT(device_is_ready(lis2mdl)); +} + +Lis2mdlDriver ::~Lis2mdlDriver() {} + +F64 Lis2mdlDriver ::sensor_value_to_f64(const struct sensor_value& val) { + return val.val1 + val.val2 / 1000000.0f; +} + +Components::MagneticField Lis2mdlDriver ::getMagneticField_handler(FwIndexType portNum) { + // Fetch new data sample from sensors + sensor_sample_fetch_chan(lis2mdl, SENSOR_CHAN_MAGN_XYZ); + + struct sensor_value x; + struct sensor_value y; + struct sensor_value z; + + sensor_channel_get(lis2mdl, SENSOR_CHAN_MAGN_X, &x); + sensor_channel_get(lis2mdl, SENSOR_CHAN_MAGN_Y, &y); + sensor_channel_get(lis2mdl, SENSOR_CHAN_MAGN_Z, &z); + + return Components::MagneticField(this->sensor_value_to_f64(x), this->sensor_value_to_f64(y), + this->sensor_value_to_f64(z)); +} + +} // namespace Components diff --git a/FprimeZephyrReference/Components/Lis2mdlDriver/Lis2mdlDriver.fpp b/FprimeZephyrReference/Components/Lis2mdlDriver/Lis2mdlDriver.fpp new file mode 100644 index 00000000..5d15f51a --- /dev/null +++ b/FprimeZephyrReference/Components/Lis2mdlDriver/Lis2mdlDriver.fpp @@ -0,0 +1,24 @@ +module Components { + @ Magnetic field reading in gauss + struct MagneticField { + x: F64 + y: F64 + z: F64 + } + + # Port definitions + port MagneticFieldRead -> MagneticField + + @ Component for F Prime FSW framework. + passive component Lis2mdlDriver { + + sync input port getMagneticField: MagneticFieldRead + + ############################################################################### + # Standard AC Ports: Required for Channels, Events, Commands, and Parameters # + ############################################################################### + @ Port for requesting the current time + time get port timeCaller + + } +} diff --git a/FprimeZephyrReference/Components/Lis2mdlDriver/Lis2mdlDriver.hpp b/FprimeZephyrReference/Components/Lis2mdlDriver/Lis2mdlDriver.hpp new file mode 100644 index 00000000..bbf289b6 --- /dev/null +++ b/FprimeZephyrReference/Components/Lis2mdlDriver/Lis2mdlDriver.hpp @@ -0,0 +1,52 @@ +// ====================================================================== +// \title Lis2mdlDriver.hpp +// \author aychar +// \brief hpp file for Lis2mdlDriver component implementation class +// ====================================================================== + +#ifndef Components_Lis2mdlDriver_HPP +#define Components_Lis2mdlDriver_HPP + +#include "FprimeZephyrReference/Components/Lis2mdlDriver/Lis2mdlDriverComponentAc.hpp" + +#include +#include +#include + +namespace Components { + +class Lis2mdlDriver final : public Lis2mdlDriverComponentBase { + public: + // ---------------------------------------------------------------------- + // Component construction and destruction + // ---------------------------------------------------------------------- + + //! Construct Lis2mdlDriver object + Lis2mdlDriver(const char* const compName //!< The component name + ); + + //! Destroy Lis2mdlDriver object + ~Lis2mdlDriver(); + + private: + //! Handler implementation + MagneticField getMagneticField_handler(FwIndexType portNum) override; + + // ---------------------------------------------------------------------- + // Helper methods + // ---------------------------------------------------------------------- + + //! Convert a Zephyr sensor_value to an Fprime F64 + F64 sensor_value_to_f64(const struct sensor_value& val); + + // ---------------------------------------------------------------------- + // Member variables + // ---------------------------------------------------------------------- + + //! Zephyr device stores the initialized LIS2MDL sensor + const struct device* lis2mdl; +}; + +} // namespace Components + +#endif diff --git a/FprimeZephyrReference/Components/Lis2mdlDriver/docs/sdd.md b/FprimeZephyrReference/Components/Lis2mdlDriver/docs/sdd.md new file mode 100644 index 00000000..fc226ca4 --- /dev/null +++ b/FprimeZephyrReference/Components/Lis2mdlDriver/docs/sdd.md @@ -0,0 +1,66 @@ +# Components::Lis2mdlDriver + +Component for F Prime FSW framework. + +## Usage Examples +Add usage examples here + +### Diagrams +Add diagrams here + +### Typical Usage +And the typical usage of the component here + +## Class Diagram +Add a class diagram here + +## Port Descriptions +| Name | Description | +|---|---| +|---|---| + +## Component States +Add component states in the chart below +| Name | Description | +|---|---| +|---|---| + +## Sequence Diagrams +Add sequence diagrams here + +## Parameters +| Name | Description | +|---|---| +|---|---| + +## Commands +| Name | Description | +|---|---| +|---|---| + +## Events +| Name | Description | +|---|---| +|---|---| + +## Telemetry +| Name | Description | +|---|---| +|---|---| + +## Unit Tests +Add unit test descriptions in the chart below +| Name | Description | Output | Coverage | +|---|---|---|---| +|---|---|---|---| + +## Requirements +Add requirements in the chart below +| Name | Description | Validation | +|---|---|---| +|---|---|---| + +## Change Log +| Date | Description | +|---|---| +|---| Initial Draft | \ No newline at end of file diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi index 1e0b66be..b4491537 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi +++ b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi @@ -40,7 +40,6 @@ telemetry packets ReferenceDeploymentPackets { ReferenceDeployment.imu.MagneticField } - } omit { CdhCore.cmdDisp.CommandErrors # Only has one library, no custom versions diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp index 1c0e018c..ca5f9bfe 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp @@ -65,4 +65,6 @@ module ReferenceDeployment { instance watchdog: Components.Watchdog base id 0x10016000 instance imu: Components.Imu base id 0x10017000 + + instance lis2mdlDriver: Components.Lis2mdlDriver base id 0x10018000 } diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp index 45563909..e286f22e 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp @@ -30,6 +30,7 @@ module ReferenceDeployment { instance watchdog instance prmDb instance imu + instance lis2mdlDriver # ---------------------------------------------------------------------- # Pattern graph specifiers @@ -100,6 +101,10 @@ module ReferenceDeployment { watchdog.gpioSet -> gpioDriver.gpioWrite } + connections Imu { + imu.readMagneticField -> lis2mdlDriver.getMagneticField + } + connections ReferenceDeployment { } From bb8a7670198d5e2ad273d36f818f5b1db1db84e4 Mon Sep 17 00:00:00 2001 From: aychar Date: Mon, 15 Sep 2025 19:06:59 -0500 Subject: [PATCH 2/2] fix formatting --- .../Components/Lis2mdlDriver/Lis2mdlDriver.fpp | 2 +- FprimeZephyrReference/Components/Lis2mdlDriver/docs/sdd.md | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/FprimeZephyrReference/Components/Lis2mdlDriver/Lis2mdlDriver.fpp b/FprimeZephyrReference/Components/Lis2mdlDriver/Lis2mdlDriver.fpp index 5d15f51a..13a2f679 100644 --- a/FprimeZephyrReference/Components/Lis2mdlDriver/Lis2mdlDriver.fpp +++ b/FprimeZephyrReference/Components/Lis2mdlDriver/Lis2mdlDriver.fpp @@ -7,7 +7,7 @@ module Components { } # Port definitions - port MagneticFieldRead -> MagneticField + port MagneticFieldRead -> MagneticField @ Component for F Prime FSW framework. passive component Lis2mdlDriver { diff --git a/FprimeZephyrReference/Components/Lis2mdlDriver/docs/sdd.md b/FprimeZephyrReference/Components/Lis2mdlDriver/docs/sdd.md index fc226ca4..7867637d 100644 --- a/FprimeZephyrReference/Components/Lis2mdlDriver/docs/sdd.md +++ b/FprimeZephyrReference/Components/Lis2mdlDriver/docs/sdd.md @@ -63,4 +63,4 @@ Add requirements in the chart below ## Change Log | Date | Description | |---|---| -|---| Initial Draft | \ No newline at end of file +|---| Initial Draft |