Skip to content

Commit

Permalink
working SHT4x driver with probing based on CRC
Browse files Browse the repository at this point in the history
  • Loading branch information
@roman-dvorak
roman-dvorak committed Aug 3, 2024
1 parent 711e544 commit ed2e784
Show file tree
Hide file tree
Showing 2 changed files with 98 additions and 64 deletions.
153 changes: 93 additions & 60 deletions src/drivers/hygrometer/sht4x/sht4x.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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{};
Expand All @@ -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()
Expand All @@ -153,49 +189,47 @@ 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;
}



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;
}
}
// }
// }



Expand All @@ -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);

}


Expand Down Expand Up @@ -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");

}

Expand Down Expand Up @@ -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;
Expand Down
9 changes: 5 additions & 4 deletions src/drivers/hygrometer/sht4x/sht4x.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -87,17 +87,18 @@ class SHT4X : public device::I2C, public ModuleParams, public I2CSPIDriver<SHT4X
int init_sensor();
void print_status() override;

void action_cli(const BusCLIArguments &cli);
//void action_cli(const BusCLIArguments &cli);


int read_data(uint8_t command, uint8_t *data_ptr, uint8_t length);
//int read_data(uint8_t command, uint8_t *data_ptr, uint8_t length);
int write_data(uint8_t command, uint8_t buffer[], uint8_t length);

int read_serialnumber();
uint8_t read_serialnumber();

void sensor_compouse_msg(bool send);
uint8_t sensor_compouse_msg(bool send);

uint8_t calc_crc(uint8_t data[2]);
uint8_t validate_crc(uint8_t data[6]);

private:

Expand Down

0 comments on commit ed2e784

Please sign in to comment.