mirror of https://github.com/ArduPilot/ardupilot
66 lines
1.9 KiB
C++
66 lines
1.9 KiB
C++
#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
|