diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_MCP9600.cpp b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_MCP9600.cpp index a9e2d8eb3c..c0c91382ce 100644 --- a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_MCP9600.cpp +++ b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_MCP9600.cpp @@ -17,7 +17,7 @@ * Code by Tom Pittenger */ /* - Implements I2C driver for Microchip MCP9600 digital thermocouple EMF to Temperature converter + Implements I2C driver for Microchip MCP9600 and MCP9601 digital thermocouple EMF to Temperature converter */ #include "AP_TemperatureSensor_MCP9600.h" @@ -35,13 +35,15 @@ static const uint8_t MCP9600_CMD_HOT_JUNCT_TEMP = 0x00; // thermocoupler //static const uint8_t MCP9600_CMD_JUNCT_TEMP_DELTA = 0x01; //static const uint8_t MCP9600_CMD_COLD_JUNCT_TEMP = 0x02; // ambient temp //static const uint8_t MCP9600_CMD_RAW_DATA_ADC = 0x03; -//static const uint8_t MCP9600_CMD_STATUS = 0x04; +static const uint8_t MCP9600_CMD_STATUS = 0x04; static const uint8_t MCP9600_CMD_SENSOR_CFG = 0x05; //static const uint8_t MCP9600_CMD_DEVICE_CFG = 0x06; static const uint8_t MCP9600_CMD_DEVICE_ID_REV = 0x20; // to fetch WHOAMI +static const uint8_t MCP9600_CMD_STATUS_UPDATE_READY = 0x40; static const uint8_t MCP9600_WHOAMI = 0x40; +static const uint8_t MCP9601_WHOAMI = 0x41; #define MCP9600_ADDR_LOW 0x60 // ADDR pin pulled low @@ -69,14 +71,13 @@ void AP_TemperatureSensor_MCP9600::init() #if AP_TemperatureSensor_MCP9600_ENFORCE_KNOWN_VALID_I2C_ADDRESS // I2C Address: Default to using MCP9600_ADDR_LOW if it's out of range if ((_params.bus_address < MCP9600_ADDR_LOW) || ( _params.bus_address > MCP9600_ADDR_HIGH)) { - printf("%s wrong I2C addr of 0x%2X, setting to 0x%2X. Reboot needed.", name, (unsigned)_params.bus_address.get(), (unsigned)MCP9600_ADDR_LOW); _params.bus_address.set(MCP9600_ADDR_LOW); } #endif _dev = std::move(hal.i2c_mgr->get_device(_params.bus, _params.bus_address)); if (!_dev) { - printf("%s device is null!", name); + // device not found return; } @@ -84,13 +85,13 @@ void AP_TemperatureSensor_MCP9600::init() _dev->set_retries(10); - uint8_t buf[2]; - if (!_dev->read_registers(MCP9600_CMD_DEVICE_ID_REV, buf, 2)) { + uint8_t buf; + if (!_dev->read_registers(MCP9600_CMD_DEVICE_ID_REV, &buf, 1)) { printf("%s failed to get WHOAMI", name); return; } - if (buf[0] != MCP9600_WHOAMI) { - printf("%s Got wrong WHOAMI: detected 0x%2X but expected 0x%2X", name, (unsigned)buf[0], (unsigned)MCP9600_WHOAMI); + if (buf != MCP9600_WHOAMI && buf != MCP9601_WHOAMI) { + printf("%s Got wrong WHOAMI: detected 0x%2X", name, (unsigned)buf); return; } if (!set_config(ThermocoupleType::K, AP_TemperatureSensor_MCP9600_Filter)) { @@ -98,10 +99,6 @@ void AP_TemperatureSensor_MCP9600::init() return; } -#if 0 - printf("%s Detected! Rev %u.%u on bus: %u addr: 0x%2X", name, (unsigned)(buf[1] >> 4), (unsigned)(buf[1] & 0x0F), (unsigned)_params.bus, (unsigned)_params.bus_address); -#endif - // lower retries for run _dev->set_retries(3); @@ -129,12 +126,29 @@ bool AP_TemperatureSensor_MCP9600::set_config(const ThermocoupleType thermoType, bool AP_TemperatureSensor_MCP9600::read_temperature(float &temperature) { + // confirm new temperature is available and then read it + + // First, read STATUS register: uint8_t data[2]; + if (!_dev->read_registers(MCP9600_CMD_STATUS, data, 1)) { + return false; + } + // check UPDATE bit for new temperature data ready: + if ((data[0] & MCP9600_CMD_STATUS_UPDATE_READY) == 0) { + return false; + } + // clear update bit: + if (!_dev->write_register(0x04, data[0] & ~MCP9600_CMD_STATUS_UPDATE_READY)) { + return false; + } + // read temperature: if (!_dev->read_registers(MCP9600_CMD_HOT_JUNCT_TEMP, data, 2)) { return false; } + // scale temperature: temperature = int16_t(UINT16_VALUE(data[0], data[1])) * AP_TemperatureSensor_MCP9600_SCALE_FACTOR; + return true; } #endif // AP_TEMPERATURE_SENSOR_MCP9600_ENABLED