diff --git a/src/drivers/temperature_sensor/mcp9808/mcp9808.h b/src/drivers/temperature_sensor/mcp9808/mcp9808.h index c26320b589..907b07db77 100644 --- a/src/drivers/temperature_sensor/mcp9808/mcp9808.h +++ b/src/drivers/temperature_sensor/mcp9808/mcp9808.h @@ -70,6 +70,8 @@ private: uORB::PublicationMulti _to_sensor_temp{ORB_ID(sensor_temp)}; perf_counter_t _cycle_perf; + perf_counter_t _comms_errors; + sensor_temp_s _sensor_temp{}; diff --git a/src/drivers/temperature_sensor/mcp9808/mcp9808_main.cpp b/src/drivers/temperature_sensor/mcp9808/mcp9808_main.cpp index 1a0c619876..d58a80001e 100644 --- a/src/drivers/temperature_sensor/mcp9808/mcp9808_main.cpp +++ b/src/drivers/temperature_sensor/mcp9808/mcp9808_main.cpp @@ -44,7 +44,9 @@ MCP9808::MCP9808(const I2CSPIDriverConfig &config) : I2C(config), I2CSPIDriver(config), - _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": single-sample")) + _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": single-sample")), + _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms errors")) + { _sensor_temp.device_id = this->get_device_id(); @@ -54,6 +56,8 @@ MCP9808::~MCP9808() { ScheduleClear(); perf_free(_cycle_perf); + perf_free(_comms_errors); + } void MCP9808::exit_and_cleanup() @@ -71,11 +75,20 @@ void MCP9808::RunImpl() measurement_time = hrt_absolute_time(); // get the time the measurement was taken float temperature = read_temperature(); - _sensor_temp.timestamp = hrt_absolute_time(); - _sensor_temp.timestamp_sample = measurement_time; - _sensor_temp.temperature = temperature; + if (std::isnan(temperature)) { + + perf_count(_comms_errors); + + } else { + + _sensor_temp.timestamp = hrt_absolute_time(); + _sensor_temp.timestamp_sample = measurement_time; + _sensor_temp.temperature = temperature; + + _to_sensor_temp.publish(_sensor_temp); + } + - _to_sensor_temp.publish(_sensor_temp); } perf_end(_cycle_perf); @@ -95,6 +108,8 @@ void MCP9808::print_status() { I2CSPIDriverBase::print_status(); perf_print_counter(_cycle_perf); + perf_print_counter(_comms_errors); + } extern "C" int mcp9808_main(int argc, char *argv[])