|
| 1 | +// ====================================================================== |
| 2 | +// \title Lis2mdlManager.cpp |
| 3 | +// \brief cpp file for Lis2mdlManager component implementation class |
| 4 | +// ====================================================================== |
| 5 | + |
| 6 | +#include "FprimeZephyrReference/Components/Drv/Lis2mdlManager/Lis2mdlManager.hpp" |
| 7 | + |
| 8 | +#include <Fw/Types/Assert.hpp> |
| 9 | + |
| 10 | +namespace Drv { |
| 11 | + |
| 12 | +// ---------------------------------------------------------------------- |
| 13 | +// Component construction and destruction |
| 14 | +// ---------------------------------------------------------------------- |
| 15 | + |
| 16 | +Lis2mdlManager ::Lis2mdlManager(const char* const compName) : Lis2mdlManagerComponentBase(compName) { |
| 17 | + dev = device_get_binding("LIS2MDL"); |
| 18 | +} |
| 19 | + |
| 20 | +Lis2mdlManager ::~Lis2mdlManager() {} |
| 21 | + |
| 22 | +// ---------------------------------------------------------------------- |
| 23 | +// Handler implementations for typed input ports |
| 24 | +// ---------------------------------------------------------------------- |
| 25 | + |
| 26 | +Drv::MagneticField Lis2mdlManager ::magneticFieldGet_handler(FwIndexType portNum) { |
| 27 | + if (!device_is_ready(dev)) { |
| 28 | + this->log_WARNING_HI_DeviceNotReady(); |
| 29 | + return Drv::MagneticField(0.0, 0.0, 0.0); |
| 30 | + } |
| 31 | + this->log_WARNING_HI_DeviceNotReady_ThrottleClear(); |
| 32 | + |
| 33 | + struct sensor_value x; |
| 34 | + struct sensor_value y; |
| 35 | + struct sensor_value z; |
| 36 | + |
| 37 | + sensor_sample_fetch_chan(dev, SENSOR_CHAN_MAGN_XYZ); |
| 38 | + |
| 39 | + sensor_channel_get(dev, SENSOR_CHAN_MAGN_X, &x); |
| 40 | + sensor_channel_get(dev, SENSOR_CHAN_MAGN_Y, &y); |
| 41 | + sensor_channel_get(dev, SENSOR_CHAN_MAGN_Z, &z); |
| 42 | + |
| 43 | + Drv::MagneticField magnetic_readings = |
| 44 | + Drv::MagneticField(Drv::sensor_value_to_f64(x), Drv::sensor_value_to_f64(y), Drv::sensor_value_to_f64(z)); |
| 45 | + |
| 46 | + this->tlmWrite_MagneticField(magnetic_readings); |
| 47 | + |
| 48 | + return magnetic_readings; |
| 49 | +} |
| 50 | + |
| 51 | +} // namespace Drv |
0 commit comments