Skip to content

Commit e44fa63

Browse files
hrfarmerhrfarmer
authored
Add LIS2MDL driver & implement in Imu component (#16)
* add lis2mdl driver and implement in imu component * fix formatting
1 parent 981de3e commit e44fa63

File tree

12 files changed

+236
-36
lines changed

12 files changed

+236
-36
lines changed

‎FprimeZephyrReference/Components/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -3,3 +3,4 @@
33
add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/FatalHandler")
44
add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/Watchdog")
55
add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/Imu/")
6+
add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/Lis2mdlDriver/")

‎FprimeZephyrReference/Components/Imu/Imu.cpp‎

Lines changed: 1 addition & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -14,10 +14,6 @@ namespace Components {
1414
// ----------------------------------------------------------------------
1515

1616
Imu ::Imu(const char* const compName) : ImuComponentBase(compName) {
17-
// Initialize the LIS2MDL sensor
18-
lis2mdl = device_get_binding("LIS2MDL");
19-
FW_ASSERT(device_is_ready(lis2mdl));
20-
2117
// Initialize the LSM6DSO sensor
2218
lsm6dso = DEVICE_DT_GET_ONE(st_lsm6dso);
2319
FW_ASSERT(device_is_ready(lsm6dso));
@@ -38,13 +34,12 @@ void Imu ::run_handler(FwIndexType portNum, U32 context) {
3834
// Fetch new data samples from the sensors
3935
sensor_sample_fetch_chan(lsm6dso, SENSOR_CHAN_ACCEL_XYZ);
4036
sensor_sample_fetch_chan(lsm6dso, SENSOR_CHAN_GYRO_XYZ);
41-
sensor_sample_fetch_chan(lis2mdl, SENSOR_CHAN_MAGN_XYZ);
4237
sensor_sample_fetch_chan(lsm6dso, SENSOR_CHAN_DIE_TEMP);
4338

4439
// Output sensor values via telemetry
4540
this->tlmWrite_Acceleration(this->get_acceleration());
4641
this->tlmWrite_AngularVelocity(this->get_angular_velocity());
47-
this->tlmWrite_MagneticField(this->get_magnetic_field());
42+
this->tlmWrite_MagneticField(this->readMagneticField_out(0));
4843
this->tlmWrite_Temperature(this->get_temperature());
4944
}
5045

@@ -78,19 +73,6 @@ Components::Imu_AngularVelocity Imu ::get_angular_velocity() {
7873
this->sensor_value_to_f64(z));
7974
}
8075

81-
Components::Imu_MagneticField Imu ::get_magnetic_field() {
82-
struct sensor_value x;
83-
struct sensor_value y;
84-
struct sensor_value z;
85-
86-
sensor_channel_get(lis2mdl, SENSOR_CHAN_MAGN_X, &x);
87-
sensor_channel_get(lis2mdl, SENSOR_CHAN_MAGN_Y, &y);
88-
sensor_channel_get(lis2mdl, SENSOR_CHAN_MAGN_Z, &z);
89-
90-
return Components::Imu_MagneticField(this->sensor_value_to_f64(x), this->sensor_value_to_f64(y),
91-
this->sensor_value_to_f64(z));
92-
}
93-
9476
F64 Imu ::get_temperature() {
9577
struct sensor_value temp;
9678

‎FprimeZephyrReference/Components/Imu/Imu.fpp‎

Lines changed: 5 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,8 @@ module Components {
33
passive component Imu {
44
sync input port run: Svc.Sched
55

6+
output port readMagneticField: MagneticFieldRead
7+
68
@ Acceleration reading in m/s^2
79
struct Acceleration {
810
x: F64
@@ -13,6 +15,9 @@ module Components {
1315
@ Telemetry channel for acceleration
1416
telemetry Acceleration: Acceleration
1517

18+
@ Telemetry channel for magnetic field
19+
telemetry MagneticField: MagneticField
20+
1621
@ Angular velocity reading in rad/s
1722
struct AngularVelocity {
1823
x: F64
@@ -23,16 +28,6 @@ module Components {
2328
@ Telemetry channel for angular velocity
2429
telemetry AngularVelocity: AngularVelocity
2530

26-
@ Magnetic field reading in gauss
27-
struct MagneticField {
28-
x: F64
29-
y: F64
30-
z: F64
31-
}
32-
33-
@ Telemetry channel for magnetic field
34-
telemetry MagneticField: MagneticField
35-
3631
@ Telemetry channel for temperature in degrees Celsius
3732
telemetry Temperature: F64
3833

‎FprimeZephyrReference/Components/Imu/Imu.hpp‎

Lines changed: 0 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -53,19 +53,13 @@ class Imu final : public ImuComponentBase {
5353
//! Get the angular velocity reading from the IMU
5454
Components::Imu_AngularVelocity get_angular_velocity();
5555

56-
//! Get the magnetic field reading from the IMU
57-
Components::Imu_MagneticField get_magnetic_field();
58-
5956
//! Get the temperature reading from the IMU
6057
F64 get_temperature();
6158

6259
// ----------------------------------------------------------------------
6360
// Member variables
6461
// ----------------------------------------------------------------------
6562

66-
//! Zephyr device stores the initialized LIS2MDL sensor
67-
const struct device* lis2mdl;
68-
6963
//! Zephyr device stores the initialized LSM6DSO sensor
7064
const struct device* lsm6dso;
7165
};

‎FprimeZephyrReference/Components/Lis2mdlDriver/CMakeLists.txt‎

Lines changed: 36 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
####
2+
# F Prime CMakeLists.txt:
3+
#
4+
# SOURCES: list of source files (to be compiled)
5+
# AUTOCODER_INPUTS: list of files to be passed to the autocoders
6+
# DEPENDS: list of libraries that this module depends on
7+
#
8+
# More information in the F´ CMake API documentation:
9+
# https://fprime.jpl.nasa.gov/latest/docs/reference/api/cmake/API/
10+
#
11+
####
12+
13+
# Module names are derived from the path from the nearest project/library/framework
14+
# root when not specifically overridden by the developer. i.e. The module defined by
15+
# `Ref/SignalGen/CMakeLists.txt` will be named `Ref_SignalGen`.
16+
17+
register_fprime_library(
18+
AUTOCODER_INPUTS
19+
"${CMAKE_CURRENT_LIST_DIR}/Lis2mdlDriver.fpp"
20+
SOURCES
21+
"${CMAKE_CURRENT_LIST_DIR}/Lis2mdlDriver.cpp"
22+
# DEPENDS
23+
# MyPackage_MyOtherModule
24+
)
25+
26+
### Unit Tests ###
27+
# register_fprime_ut(
28+
# AUTOCODER_INPUTS
29+
# "${CMAKE_CURRENT_LIST_DIR}/Lis2mdlDriver.fpp"
30+
# SOURCES
31+
# "${CMAKE_CURRENT_LIST_DIR}/test/ut/Lis2mdlDriverTestMain.cpp"
32+
# "${CMAKE_CURRENT_LIST_DIR}/test/ut/Lis2mdlDriverTester.cpp"
33+
# DEPENDS
34+
# STest # For rules-based testing
35+
# UT_AUTO_HELPERS
36+
# )

‎FprimeZephyrReference/Components/Lis2mdlDriver/Lis2mdlDriver.cpp‎

Lines changed: 44 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,44 @@
1+
// ======================================================================
2+
// \title Lis2mdlDriver.cpp
3+
// \author aychar
4+
// \brief cpp file for Lis2mdlDriver component implementation class
5+
// ======================================================================
6+
7+
#include "FprimeZephyrReference/Components/Lis2mdlDriver/Lis2mdlDriver.hpp"
8+
9+
#include <Fw/Types/Assert.hpp>
10+
11+
namespace Components {
12+
13+
// ----------------------------------------------------------------------
14+
// Component construction and destruction
15+
// ----------------------------------------------------------------------
16+
17+
Lis2mdlDriver ::Lis2mdlDriver(const char* const compName) : Lis2mdlDriverComponentBase(compName) {
18+
lis2mdl = device_get_binding("LIS2MDL");
19+
FW_ASSERT(device_is_ready(lis2mdl));
20+
}
21+
22+
Lis2mdlDriver ::~Lis2mdlDriver() {}
23+
24+
F64 Lis2mdlDriver ::sensor_value_to_f64(const struct sensor_value& val) {
25+
return val.val1 + val.val2 / 1000000.0f;
26+
}
27+
28+
Components::MagneticField Lis2mdlDriver ::getMagneticField_handler(FwIndexType portNum) {
29+
// Fetch new data sample from sensors
30+
sensor_sample_fetch_chan(lis2mdl, SENSOR_CHAN_MAGN_XYZ);
31+
32+
struct sensor_value x;
33+
struct sensor_value y;
34+
struct sensor_value z;
35+
36+
sensor_channel_get(lis2mdl, SENSOR_CHAN_MAGN_X, &x);
37+
sensor_channel_get(lis2mdl, SENSOR_CHAN_MAGN_Y, &y);
38+
sensor_channel_get(lis2mdl, SENSOR_CHAN_MAGN_Z, &z);
39+
40+
return Components::MagneticField(this->sensor_value_to_f64(x), this->sensor_value_to_f64(y),
41+
this->sensor_value_to_f64(z));
42+
}
43+
44+
} // namespace Components

‎FprimeZephyrReference/Components/Lis2mdlDriver/Lis2mdlDriver.fpp‎

Lines changed: 24 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,24 @@
1+
module Components {
2+
@ Magnetic field reading in gauss
3+
struct MagneticField {
4+
x: F64
5+
y: F64
6+
z: F64
7+
}
8+
9+
# Port definitions
10+
port MagneticFieldRead -> MagneticField
11+
12+
@ Component for F Prime FSW framework.
13+
passive component Lis2mdlDriver {
14+
15+
sync input port getMagneticField: MagneticFieldRead
16+
17+
###############################################################################
18+
# Standard AC Ports: Required for Channels, Events, Commands, and Parameters #
19+
###############################################################################
20+
@ Port for requesting the current time
21+
time get port timeCaller
22+
23+
}
24+
}

‎FprimeZephyrReference/Components/Lis2mdlDriver/Lis2mdlDriver.hpp‎

Lines changed: 52 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,52 @@
1+
// ======================================================================
2+
// \title Lis2mdlDriver.hpp
3+
// \author aychar
4+
// \brief hpp file for Lis2mdlDriver component implementation class
5+
// ======================================================================
6+
7+
#ifndef Components_Lis2mdlDriver_HPP
8+
#define Components_Lis2mdlDriver_HPP
9+
10+
#include "FprimeZephyrReference/Components/Lis2mdlDriver/Lis2mdlDriverComponentAc.hpp"
11+
12+
#include <zephyr/device.h>
13+
#include <zephyr/drivers/sensor.h>
14+
#include <zephyr/kernel.h>
15+
16+
namespace Components {
17+
18+
class Lis2mdlDriver final : public Lis2mdlDriverComponentBase {
19+
public:
20+
// ----------------------------------------------------------------------
21+
// Component construction and destruction
22+
// ----------------------------------------------------------------------
23+
24+
//! Construct Lis2mdlDriver object
25+
Lis2mdlDriver(const char* const compName //!< The component name
26+
);
27+
28+
//! Destroy Lis2mdlDriver object
29+
~Lis2mdlDriver();
30+
31+
private:
32+
//! Handler implementation
33+
MagneticField getMagneticField_handler(FwIndexType portNum) override;
34+
35+
// ----------------------------------------------------------------------
36+
// Helper methods
37+
// ----------------------------------------------------------------------
38+
39+
//! Convert a Zephyr sensor_value to an Fprime F64
40+
F64 sensor_value_to_f64(const struct sensor_value& val);
41+
42+
// ----------------------------------------------------------------------
43+
// Member variables
44+
// ----------------------------------------------------------------------
45+
46+
//! Zephyr device stores the initialized LIS2MDL sensor
47+
const struct device* lis2mdl;
48+
};
49+
50+
} // namespace Components
51+
52+
#endif

‎FprimeZephyrReference/Components/Lis2mdlDriver/docs/sdd.md‎

Lines changed: 66 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,66 @@
1+
# Components::Lis2mdlDriver
2+
3+
Component for F Prime FSW framework.
4+
5+
## Usage Examples
6+
Add usage examples here
7+
8+
### Diagrams
9+
Add diagrams here
10+
11+
### Typical Usage
12+
And the typical usage of the component here
13+
14+
## Class Diagram
15+
Add a class diagram here
16+
17+
## Port Descriptions
18+
| Name | Description |
19+
|---|---|
20+
|---|---|
21+
22+
## Component States
23+
Add component states in the chart below
24+
| Name | Description |
25+
|---|---|
26+
|---|---|
27+
28+
## Sequence Diagrams
29+
Add sequence diagrams here
30+
31+
## Parameters
32+
| Name | Description |
33+
|---|---|
34+
|---|---|
35+
36+
## Commands
37+
| Name | Description |
38+
|---|---|
39+
|---|---|
40+
41+
## Events
42+
| Name | Description |
43+
|---|---|
44+
|---|---|
45+
46+
## Telemetry
47+
| Name | Description |
48+
|---|---|
49+
|---|---|
50+
51+
## Unit Tests
52+
Add unit test descriptions in the chart below
53+
| Name | Description | Output | Coverage |
54+
|---|---|---|---|
55+
|---|---|---|---|
56+
57+
## Requirements
58+
Add requirements in the chart below
59+
| Name | Description | Validation |
60+
|---|---|---|
61+
|---|---|---|
62+
63+
## Change Log
64+
| Date | Description |
65+
|---|---|
66+
|---| Initial Draft |

‎FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -40,7 +40,6 @@ telemetry packets ReferenceDeploymentPackets {
4040
ReferenceDeployment.imu.MagneticField
4141
}
4242

43-
4443
} omit {
4544
CdhCore.cmdDisp.CommandErrors
4645
# Only has one library, no custom versions

0 commit comments

Comments
 (0)