Acceleration and AngularVelocity telemetry working

Author: nateinaction

FprimeZephyrReference/Components/Imu/Imu.cpp

@@ -15,9 +15,18 @@ namespace Components {
 // ----------------------------------------------------------------------
 
 Imu ::Imu(const char* const compName) : ImuComponentBase(compName) {
+    // Initialize the LIS2MDL sensor
     lis2mdl = device_get_binding("LIS2MDL");
-    FW_ASSERT(lis2mdl != nullptr);
     FW_ASSERT(device_is_ready(lis2mdl));
+
+    // Initialize the LSM6DSO sensor
+    lsm6dso = DEVICE_DT_GET_ONE(st_lsm6dso);
+    FW_ASSERT(device_is_ready(lsm6dso));
+
+    // Configure the LSM6DSO sensor
+    struct sensor_value odr = {.val1 = 12, .val2 = 500000};  // 12.5 Hz
+    FW_ASSERT(sensor_attr_set(lsm6dso, SENSOR_CHAN_ACCEL_XYZ, SENSOR_ATTR_SAMPLING_FREQUENCY, &odr) == 0);
+    FW_ASSERT(sensor_attr_set(lsm6dso, SENSOR_CHAN_GYRO_XYZ, SENSOR_ATTR_SAMPLING_FREQUENCY, &odr) == 0);
 }
 
 Imu ::~Imu() {}
@@ -28,24 +37,54 @@ Imu ::~Imu() {}
 
 void Imu ::run_handler(FwIndexType portNum, U32 context) {
     // Fetch new data sample
+    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);
 
-    // Extract the data from the sample
-    struct sensor_value magnetic_data_x;
-    struct sensor_value magnetic_data_y;
-    struct sensor_value magnetic_data_z;
-    sensor_channel_get(lis2mdl, SENSOR_CHAN_MAGN_X, &magnetic_data_x);
-    sensor_channel_get(lis2mdl, SENSOR_CHAN_MAGN_Y, &magnetic_data_y);
-    sensor_channel_get(lis2mdl, SENSOR_CHAN_MAGN_Z, &magnetic_data_z);
+    // Output the magnetic field values via telemetry
+    this->tlmWrite_Acceleration(this->get_acceleration());
+    this->tlmWrite_AngularVelocity(this->get_angular_velocity());
+    this->tlmWrite_MagneticField(this->get_magnetic_field());
+}
 
-    // Convert to float values in gauss
-    float magnetic_field_x = magnetic_data_x.val1 + magnetic_data_x.val2 / 1000000.0f;
-    float magnetic_field_y = magnetic_data_y.val1 + magnetic_data_y.val2 / 1000000.0f;
-    float magnetic_field_z = magnetic_data_z.val1 + magnetic_data_z.val2 / 1000000.0f;
+F64 Imu ::sensor_value_to_f64(const struct sensor_value& val) {
+    return val.val1 + val.val2 / 1000000.0f;
+}
 
-    // Output the magnetic field values via telemetry
-    this->tlmWrite_MagneticField(Components::Imu_MagneticField(
-        static_cast<F64>(magnetic_field_x), static_cast<F64>(magnetic_field_y), static_cast<F64>(magnetic_field_z)));
+Components::Imu_Acceleration Imu ::get_acceleration() {
+    struct sensor_value x;
+    struct sensor_value y;
+    struct sensor_value z;
+    sensor_channel_get(lsm6dso, SENSOR_CHAN_ACCEL_X, &x);
+    sensor_channel_get(lsm6dso, SENSOR_CHAN_ACCEL_Y, &y);
+    sensor_channel_get(lsm6dso, SENSOR_CHAN_ACCEL_Z, &z);
+
+    return Components::Imu_Acceleration(this->sensor_value_to_f64(x), this->sensor_value_to_f64(y),
+                                        this->sensor_value_to_f64(z));
+}
+
+Components::Imu_AngularVelocity Imu ::get_angular_velocity() {
+    struct sensor_value x;
+    struct sensor_value y;
+    struct sensor_value z;
+    sensor_channel_get(lsm6dso, SENSOR_CHAN_GYRO_X, &x);
+    sensor_channel_get(lsm6dso, SENSOR_CHAN_GYRO_Y, &y);
+    sensor_channel_get(lsm6dso, SENSOR_CHAN_GYRO_Z, &z);
+
+    return Components::Imu_AngularVelocity(this->sensor_value_to_f64(x), this->sensor_value_to_f64(y),
+                                           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));
 }
 
 }  // namespace Components

FprimeZephyrReference/Components/Imu/Imu.hpp

@@ -40,7 +40,14 @@ class Imu final : public ImuComponentBase {
                      U32 context           //!< The call order
                      ) override;
 
+    F64 sensor_value_to_f64(const struct sensor_value& val);
+
+    Components::Imu_Acceleration get_acceleration();
+    Components::Imu_AngularVelocity get_angular_velocity();
+    Components::Imu_MagneticField get_magnetic_field();
+
     const struct device* lis2mdl;
+    const struct device* lsm6dso;
 };
 
 }  // namespace Components

FprimeZephyrReference/Components/Imu/docs/sdd.md

@@ -63,4 +63,4 @@ Add requirements in the chart below
 ## Change Log
 | Date | Description |
 |---|---|
-|---| Initial Draft |
+|---| Initial Draft |