AP_TemperatureSensor: new MLX90614 sensor backend driver added

ArduPilot · Aug 20, 2024 · e2d8940 · e2d8940
1 parent b86380b
commit e2d8940
Show file tree

Hide file tree

Showing 8 changed files with 105 additions and 1 deletion.
diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py
@@ -318,6 +318,7 @@ def __init__(self,
     Feature('Sensors', 'TEMP_TSYS01', 'AP_TEMPERATURE_SENSOR_TSYS01_ENABLED', 'Enable Temp Sensor - TSYS01', 0, "TEMP"),
     Feature('Sensors', 'TEMP_MCP9600', 'AP_TEMPERATURE_SENSOR_MCP9600_ENABLED', 'Enable Temp Sensor - MCP9600', 0, "TEMP"),
     Feature('Sensors', 'TEMP_TSYS03', 'AP_TEMPERATURE_SENSOR_TSYS03_ENABLED', 'Enable Temp Sensor - TSYS03', 0, "TEMP"),
+    Feature('Sensors', 'TEMP_MLX90614', 'AP_TEMPERATURE_SENSOR_MLX90614_ENABLED', 'Enable Temp Sensor - MLX90614', 0, "TEMP"),
 
     Feature('Sensors', 'AIRSPEED', 'AP_AIRSPEED_ENABLED', 'Enable Airspeed Sensors', 1, None),    # Default to enabled to not annoy Plane users   # NOQA: E501
     Feature('Sensors', 'BEACON', 'AP_BEACON_ENABLED', 'Enable Beacon', 0, None),

diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor.cpp b/libraries/AP_TemperatureSensor/AP_TemperatureSensor.cpp
@@ -30,6 +30,7 @@
 #include "AP_TemperatureSensor_MAX31865.h"
 #include "AP_TemperatureSensor_Analog.h"
 #include "AP_TemperatureSensor_DroneCAN.h"
+#include "AP_TemperatureSensor_MLX90614.h"
 
 #include <AP_Logger/AP_Logger.h>
 #include <AP_Vehicle/AP_Vehicle_Type.h>
@@ -203,6 +204,11 @@ void AP_TemperatureSensor::init()
             case AP_TemperatureSensor_Params::Type::DRONECAN:
                 drivers[instance] = NEW_NOTHROW AP_TemperatureSensor_DroneCAN(*this, _state[instance], _params[instance]);
                 break;
+#endif
+#if AP_TEMPERATURE_SENSOR_MLX90614_ENABLED
+            case AP_TemperatureSensor_Params::Type::MLX90614:
+                drivers[instance] = new AP_TemperatureSensor_MLX90614(*this, _state[instance], _params[instance]);
+                break;
 #endif
             case AP_TemperatureSensor_Params::Type::NONE:
             default:

diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor.h b/libraries/AP_TemperatureSensor/AP_TemperatureSensor.h
@@ -27,6 +27,7 @@ class AP_TemperatureSensor_MCP9600;
 class AP_TemperatureSensor_MAX31865;
 class AP_TemperatureSensor_TSYS03;
 class AP_TemperatureSensor_Analog;
+class AP_TemperatureSensor_MLX90614;
 
 class AP_TemperatureSensor
 {
@@ -37,6 +38,7 @@ class AP_TemperatureSensor
     friend class AP_TemperatureSensor_TSYS03;
     friend class AP_TemperatureSensor_Analog;
     friend class AP_TemperatureSensor_DroneCAN;
+    friend class AP_TemperatureSensor_MLX90614;
 
 public:
 

diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_MLX90614.cpp b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_MLX90614.cpp
@@ -0,0 +1,66 @@
+#include "AP_TemperatureSensor_config.h"
+
+#if AP_TEMPERATURE_SENSOR_MLX90614_ENABLED
+
+#include "AP_TemperatureSensor_MLX90614.h"
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_HAL/I2CDevice.h>
+#include <AP_Math/AP_Math.h>
+
+
+extern const AP_HAL::HAL &hal;
+
+#define MLX90614_I2CDEFAULTADDR 0x5A    // Device default slave address 
+#define MLX90614_BROADCASTADDR  0       // Device broadcast slave address 
+
+//  RAM addresses
+#define MLX90614_RAWIR1         0x04    //  RAM reg - Raw temperature, source #1 
+#define MLX90614_RAWIR2         0x05    //  RAM reg - Raw temperature, source #2 
+#define MLX90614_TA             0x06    //  RAM reg - Linearized temperature, ambient 
+#define MLX90614_TOBJ1          0x07    //  RAM reg - Linearized temperature, source #1 
+#define MLX90614_TOBJ2          0x08    //  RAM reg - Linearized temperature, source #2 
+
+void AP_TemperatureSensor_MLX90614::init()
+{
+    _params.bus_address.set_default(MLX90614_I2CDEFAULTADDR);
+
+    _dev = std::move(hal.i2c_mgr->get_device(_params.bus, _params.bus_address));
+    if (!_dev) {
+        return;
+    }
+
+    WITH_SEMAPHORE(_dev->get_semaphore());
+
+    _dev->register_periodic_callback(50 * AP_USEC_PER_MSEC,
+                                     FUNCTOR_BIND_MEMBER(&AP_TemperatureSensor_MLX90614::_timer, void));
+}
+
+
+void AP_TemperatureSensor_MLX90614::_timer()
+{
+    const uint16_t _crude_value = read_data(MLX90614_TA);
+
+    if (_crude_value == 0) {
+        return;
+    }
+
+    WITH_SEMAPHORE(_dev->get_semaphore());
+
+    // temp * 0.02 - 273.15 = degrees, temp * 0.02 is temperature in kelvin
+    const float tmp = KELVIN_TO_C(_crude_value) * 0.02;
+    set_temperature(tmp);
+}
+
+
+
+uint16_t AP_TemperatureSensor_MLX90614::read_data(uint8_t cmd)
+{
+    uint8_t val[3];
+
+    if (!_dev->transfer(&cmd, 1, val, 3)) {
+        return 0;
+    }
+    return UINT16_VALUE(val[1],val[0]);
+}
+#endif // AP_TEMPERATURE_SENSOR_MLX90614_ENABLED
diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_MLX90614.h b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_MLX90614.h
@@ -0,0 +1,24 @@
+#pragma once
+
+#include "AP_TemperatureSensor_Backend.h"
+
+#if AP_TEMPERATURE_SENSOR_MLX90614_ENABLED
+
+
+class AP_TemperatureSensor_MLX90614 : public AP_TemperatureSensor_Backend {
+    using AP_TemperatureSensor_Backend::AP_TemperatureSensor_Backend;
+public:
+
+    void init(void) override;
+
+    void update() override {};
+
+private:
+
+    // update the temperature, called at 20Hz
+    void _timer(void);
+
+    uint16_t read_data(uint8_t cmd);
+    uint16_t read_eeprom(uint8_t address) {return read_data(address | 0x20);};
+};
+#endif  // AP_TEMPERATURE_SENSOR_MLX90614_ENABLED
diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.cpp b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.cpp
@@ -34,7 +34,7 @@ const AP_Param::GroupInfo AP_TemperatureSensor_Params::var_info[] = {
     // @Param: TYPE
     // @DisplayName: Temperature Sensor Type
     // @Description: Enables temperature sensors
-    // @Values: 0:Disabled, 1:TSYS01, 2:MCP9600, 3:MAX31865, 4:TSYS03, 5:Analog, 6:DroneCAN
+    // @Values: 0:Disabled, 1:TSYS01, 2:MCP9600, 3:MAX31865, 4:TSYS03, 5:Analog, 6:DroneCAN, 7:MLX90614
     // @User: Standard
     // @RebootRequired: True
     AP_GROUPINFO_FLAGS("TYPE", 1, AP_TemperatureSensor_Params, type, (float)Type::NONE, AP_PARAM_FLAG_ENABLE),

diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.h b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.h
@@ -34,6 +34,7 @@ class AP_TemperatureSensor_Params {
         TSYS03                      = 4,
         ANALOG                      = 5,
         DRONECAN                    = 6,
+        MLX90614                    = 7,
     };
 
     // option to map to another system component

diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_config.h b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_config.h
@@ -35,6 +35,10 @@
     #error AP_TEMPERATURE_SENSOR_DRONECAN_ENABLED requires HAL_ENABLE_DRONECAN_DRIVERS
 #endif
 
+#ifndef AP_TEMPERATURE_SENSOR_MLX90614_ENABLED
+    #define AP_TEMPERATURE_SENSOR_MLX90614_ENABLED AP_TEMPERATURE_SENSOR_ENABLED
+#endif
+
 // maximum number of Temperature Sensors
 #ifndef AP_TEMPERATURE_SENSOR_MAX_INSTANCES
 #define AP_TEMPERATURE_SENSOR_MAX_INSTANCES             3