diff --git a/src/drivers/hygrometer/sht4x/sht4x.cpp b/src/drivers/hygrometer/sht4x/sht4x.cpp index 6e4395661ddf..869e4dd2545e 100644 --- a/src/drivers/hygrometer/sht4x/sht4x.cpp +++ b/src/drivers/hygrometer/sht4x/sht4x.cpp @@ -71,60 +71,96 @@ uint8_t SHT4X::calc_crc(uint8_t data[2]) } - - -int SHT4X::read_data(uint8_t command, uint8_t *data_ptr, uint8_t length) +uint8_t SHT4X::validate_crc(uint8_t data[6]) { - PX4_INFO("READ DATA..."); - PX4_INFO("%x", command); - - uint8_t raw_data[length/2*3]; - transfer(&command, 1, &raw_data[0], length/2*3); - - PX4_INFO("RAW DATA..."); - for (int i = 0; i < length/2*3; i++) { - PX4_INFO("> %x", raw_data[i]); - } - uint8_t crc_err = 0; - for (int i = 0; i < length / 3; ++i) { - uint8_t crc_data[2] = {raw_data[i * 3], raw_data[i * 3 + 1]}; + for (int i = 0; i < 6 / 3; ++i) { + uint8_t crc_data[2] = {data[i * 3], data[i * 3 + 1]}; - if (raw_data[i * 3 + 2] != calc_crc(crc_data)) { + if (data[i * 3 + 2] != calc_crc(crc_data)) { crc_err ++; } - *(data_ptr + i * 2) = raw_data[i * 3]; - *(data_ptr + i * 2 + 1) = raw_data[i * 3 + 1]; } return crc_err; + } -int SHT4X::read_serialnumber() + +// int SHT4X::read_data(uint8_t command, uint8_t *data_ptr, uint8_t length) +// { + +// uint8_t raw_data[length/2*3]; +// transfer(&command, 1, &raw_data[0], length/2*3); +// for (int i = 0; i < length/2*3; i++) { +// PX4_INFO("> %x", raw_data[i]); +// } + +// uint8_t crc_err = 0; +// for (int i = 0; i < length / 3; ++i) { +// uint8_t crc_data[2] = {raw_data[i * 3], raw_data[i * 3 + 1]}; + +// if (raw_data[i * 3 + 2] != calc_crc(crc_data)) { +// crc_err ++; +// } + +// *(data_ptr + i * 2) = raw_data[i * 3]; +// *(data_ptr + i * 2 + 1) = raw_data[i * 3 + 1]; +// } + +// return crc_err; +// } + + + +uint8_t SHT4X::read_serialnumber() { - int err = read_data(SHT4x_CMD_MEASURE_HIGH_PRECISION, (uint8_t *)&_sht_info.serial_number, 4); - PX4_INFO("READ SN STATUS: %d", err); + uint8_t data[6]; + uint8_t addr = SHT4x_CMD_READ_SERIAL; + transfer(&addr, 1, nullptr, 0); + px4_usleep(1000); + transfer(nullptr, 0, data, 6); + + uint8_t crc = validate_crc(data); + + if (crc == PX4_OK) { + _sht_info.serial_number = data[0] << 24 | data[1] << 16 | data[3] << 8 | data[4]; + } - return err; + return crc; } -void SHT4X::sensor_compouse_msg(bool send) +uint8_t SHT4X::sensor_compouse_msg(bool send) { - uint8_t data[4]; - int error = read_data(SHT4x_CMD_MEASURE_HIGH_PRECISION, &data[0], 4); - if (error == PX4_OK) { + uint8_t data[6]; + uint8_t addr = SHT4x_CMD_MEASURE_HIGH_PRECISION; + + transfer(&addr, 1, nullptr, 0); + px4_usleep(9000); // 9ms for high precision mode + transfer(nullptr, 0, data, 6); + + uint8_t crc_err = validate_crc(data); + + if (crc_err == PX4_OK) { measurement_time = hrt_absolute_time(); measurement_index ++; measured_temperature = (float) 175 * (data[0] << 8 | data[1]) / 65535 - 45; - measured_humidity = (float) 100 * (data[2] << 8 | data[3]) / 65535; + measured_humidity = (float) 125 * (data[3] << 8 | data[4]) / 65535 - 6; + + PX4_INFO("SHT4x: Temp: %.3f, Hum: %.3f", (double)measured_temperature, (double)measured_humidity); + + if (measured_humidity < 0) { measured_humidity = 0; } + + if (measured_humidity > 100) { measured_humidity = 100; } + if (send) { sensor_hygrometer_s msg{}; @@ -136,15 +172,15 @@ void SHT4X::sensor_compouse_msg(bool send) _sensor_hygrometer_pub.publish(msg); } } + + return crc_err; } int SHT4X::probe() { - //return read_serialnumber(); - // 0 means I can see sensor - return 0; + return read_serialnumber(); } int SHT4X::init() @@ -153,10 +189,10 @@ int SHT4X::init() return PX4_ERROR; } - read_data(SHT4x_CMD_SOFT_RESET, nullptr, 0); - px4_usleep(2000); + read_serialnumber(); + _sensor_hygrometer_pub.advertise(); - ScheduleOnInterval(20000); + ScheduleOnInterval(100000); return PX4_OK; } @@ -164,38 +200,36 @@ int SHT4X::init() int SHT4X::init_sensor() { - probe(); + //probe(); PX4_INFO("Connected to SHT4x sensor, SN: %ld", _sht_info.serial_number); + PX4_INFO("Last values, T: %.3f, H: %.3f", (double)measured_temperature, (double)measured_humidity); return PX4_OK; } void SHT4X::RunImpl() { + PX4_INFO("SHT4X: RunImpl"); sensor_compouse_msg(true); } -void -SHT4X::action_cli(const BusCLIArguments &cli) -{ - switch (cli.custom1) { +// void +// SHT4X::action_cli(const BusCLIArguments &cli) +// { +// switch (cli.custom1) { - case 1: { - PX4_INFO("Last measured values (%.3fs ago, #%d)", (double)(hrt_absolute_time() - measurement_time) / 1000000.0, - measurement_index); - PX4_INFO("Temp: %.3f, Hum: %.3f", (double)measured_temperature, (double)measured_humidity); +// case 1: { +// PX4_INFO("Last measured values (%.3fs ago, #%d)", (double)(hrt_absolute_time() - measurement_time) / 1000000.0, +// measurement_index); +// PX4_INFO("Temp: %.3f, Hum: %.3f", (double)measured_temperature, (double)measured_humidity); - } - break; +// } +// break; - case 2: { - read_data(SHT4x_CMD_SOFT_RESET, nullptr, 0); - } - break; - } -} +// } +// } @@ -204,6 +238,10 @@ void SHT4X::print_status() PX4_INFO("SHT4X sensor"); I2CSPIDriverBase::print_status(); PX4_INFO("SN: %ld", _sht_info.serial_number); + PX4_INFO("Last measured values (%.3fs ago, #%d)", (double)(hrt_absolute_time() - measurement_time) / 1000000.0, + measurement_index); + PX4_INFO("Temp: %.3f, Hum: %.3f", (double)measured_temperature, (double)measured_humidity); + } @@ -240,7 +278,6 @@ CLI usage example: PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); PRINT_MODULE_USAGE_COMMAND_DESCR("values", "Print actual data"); - PRINT_MODULE_USAGE_COMMAND_DESCR("reset", "Reinitialize sensor"); } @@ -273,15 +310,11 @@ int sht4x_main(int argc, char *argv[]) return ThisDriver::module_status(iterator); } - if (!strcmp(verb, "values")) { - cli.custom1 = 1; - return ThisDriver::module_custom_method(cli, iterator, false); - } + // if (!strcmp(verb, "values")) { + // cli.custom1 = 1; + // return ThisDriver::action_cli(cli, iterator, false); + // } - if (!strcmp(verb, "reset")) { - cli.custom1 = 2; - return ThisDriver::module_custom_method(cli, iterator, false); - } ThisDriver::print_usage(); return -1; diff --git a/src/drivers/hygrometer/sht4x/sht4x.h b/src/drivers/hygrometer/sht4x/sht4x.h index cdef9341d522..6b30f2f3b9b0 100644 --- a/src/drivers/hygrometer/sht4x/sht4x.h +++ b/src/drivers/hygrometer/sht4x/sht4x.h @@ -87,17 +87,18 @@ class SHT4X : public device::I2C, public ModuleParams, public I2CSPIDriver