diff --git a/src/eeprom/eeprom.c b/src/eeprom/eeprom.c index 47ff9e5..28fb9e4 100644 --- a/src/eeprom/eeprom.c +++ b/src/eeprom/eeprom.c @@ -1,16 +1,12 @@ #include "eeprom.h" +#include "sensor_api.h" #include #include +#include /** The address of the EEPROM on the I2C bus. */ #define EEPROM_ADDR 0x50 -/** Address for reading the EEPROM. */ -#define EEPROM_READ (EEPROM_ADDR | 0x1) - -/** Address for writing to the EEPROM. */ -#define EEPROM_WRITE (EEPROM_ADDR & 0xFE) - /** Defines a small buffer for the dummy write request. */ struct dummy_write_t { i2c_send_t header; /**< The send header containing address information. */ @@ -32,25 +28,32 @@ errno_t eeprom_read(uint8_t addr, int bus, void *buf, size_t n) { { .stop = 0, .len = 1, - .slave = {.fmt = I2C_ADDRFMT_7BIT, .addr = EEPROM_WRITE}, + .slave = {.fmt = I2C_ADDRFMT_7BIT, .addr = EEPROM_ADDR}, }, }; dummy_write.byte_address = addr; - errno_t err = devctl(bus, DCMD_I2C_SEND, &dummy_write, sizeof(dummy_write), NULL); + // Lock the bus + errno_t err = devctl(bus, DCMD_I2C_LOCK, NULL, 0, NULL); if (err != EOK) return err; + err = devctl(bus, DCMD_I2C_SEND, &dummy_write, sizeof(dummy_write), NULL); + if (err != EOK) goto return_defer; + // Start sequential read into buffer i2c_sendrecv_t read_header = { .stop = 1, .send_len = 0, .recv_len = n, - .slave = {.fmt = I2C_ADDRFMT_7BIT, .addr = EEPROM_WRITE}, + .slave = {.fmt = I2C_ADDRFMT_7BIT, .addr = EEPROM_ADDR}, }; memcpy(buf, &read_header, sizeof(read_header)); err = devctl(bus, DCMD_I2C_SENDRECV, buf, n + sizeof(read_header), NULL); - if (err != EOK) return err; - return EOK; + if (err != EOK) goto return_defer; + +return_defer: + devctl(bus, DCMD_I2C_UNLOCK, NULL, 0, NULL); // Unlock I2C bus + return err; } /** diff --git a/src/sensors/lsm6dso32/lsm6dso32.c b/src/sensors/lsm6dso32/lsm6dso32.c index 4f76e22..a597b6c 100644 --- a/src/sensors/lsm6dso32/lsm6dso32.c +++ b/src/sensors/lsm6dso32/lsm6dso32.c @@ -16,6 +16,7 @@ #include #include #include +#include #include /** Acceleration due to gravity in m/s^2. */ @@ -79,13 +80,15 @@ enum imu_reg { /** Macro to early return an error. */ #define return_err(err) \ - if (err != EOK) return err + if (err != EOK) goto return_defer; /** A list of data types that can be read by the LSM6DSO32. */ static const SensorTag TAGS[] = {TAG_TEMPERATURE, TAG_LINEAR_ACCEL, TAG_ANGULAR_VEL}; /** * Write data to a register of the LSM6DSO32. + * WARNING: This function does not lock the I2C bus. It is meant to be called multiple times and therefore the bus must + * be locked by the caller. * @param sensor A pointer to a LSM6DSO32 sensor instance. * @param reg The address of the register to write to. * @param data The byte of data to write to the register. @@ -108,6 +111,7 @@ static errno_t lsm6dso32_write_byte(Sensor const *sensor, const uint8_t reg, con /** * Read data from register address `reg`. + * WARNING: This function does not lock the I2C bus, as it is meant to be used in a continuous stream of calls. * @param sensor A reference to an LSM6DSO32 sensor instance. * @param reg The register address to read from. * @param buf The buffer to read data into. @@ -123,10 +127,10 @@ static errno_t lsm6dso32_read_byte(Sensor *sensor, uint8_t reg, uint8_t *buf) { read_cmd[sizeof(read_hdr)] = reg; // Data to be send is the register address errno_t err = devctl(sensor->loc.bus, DCMD_I2C_SENDRECV, read_cmd, sizeof(read_cmd), NULL); - return_err(err); + if (err != EOK) return err; *buf = read_cmd[sizeof(read_hdr)]; // Received byte will be the last one - return EOK; + return err; } /** @@ -140,6 +144,9 @@ static errno_t lsm6dso32_read_byte(Sensor *sensor, uint8_t reg, uint8_t *buf) { static errno_t lsm6dso32_read(Sensor *sensor, const SensorTag tag, void *buf, size_t *nbytes) { static errno_t err; + err = devctl(sensor->loc.bus, DCMD_I2C_LOCK, NULL, 0, NULL); // Lock I2C bus + if (err != EOK) return err; + switch (tag) { case TAG_TEMPERATURE: { int16_t temp; @@ -147,6 +154,8 @@ static errno_t lsm6dso32_read(Sensor *sensor, const SensorTag tag, void *buf, si return_err(err); err = lsm6dso32_read_byte(sensor, OUT_TEMP_H, (uint8_t *)(&temp) + 1); // Read high byte return_err(err); + devctl(sensor->loc.bus, DCMD_I2C_UNLOCK, NULL, 0, NULL); // Unlock bus + *(float *)buf = (float)(temp) / 256.0f + 25.0f; // In degrees Celsius *nbytes = sizeof(float); break; @@ -171,6 +180,7 @@ static errno_t lsm6dso32_read(Sensor *sensor, const SensorTag tag, void *buf, si err = lsm6dso32_read_byte(sensor, OUTZ_H_A, (uint8_t *)(&z) + 1); return_err(err); + devctl(sensor->loc.bus, DCMD_I2C_UNLOCK, NULL, 0, NULL); // Unlock bus // Converts milli-Gs per LSB to m/s^2 float conversion_factor = (float)((LSM6DSO32Context *)(sensor->context.data))->acc_fsr * MILLI_UNIT_PER_LSB_TO_UNIT * GRAVIT_ACC; @@ -201,6 +211,7 @@ static errno_t lsm6dso32_read(Sensor *sensor, const SensorTag tag, void *buf, si err = lsm6dso32_read_byte(sensor, OUTZ_H_G, (uint8_t *)(&z) + 1); return_err(err); + devctl(sensor->loc.bus, DCMD_I2C_UNLOCK, NULL, 0, NULL); // Unlock bus // Converts millidegrees per second per LSB to degrees per second float conversion_factor = (float)((LSM6DSO32Context *)(sensor->context.data))->gyro_fsr * MILLI_UNIT_PER_LSB_TO_UNIT; @@ -214,7 +225,14 @@ static errno_t lsm6dso32_read(Sensor *sensor, const SensorTag tag, void *buf, si default: return EINVAL; } - return EOK; + + return err; + +// Returning an error that requires cleanup +return_defer: + devctl(sensor->loc.bus, DCMD_I2C_UNLOCK, NULL, 0, NULL); // Unlock bus + *nbytes = 0; + return err; } /** @@ -225,7 +243,10 @@ static errno_t lsm6dso32_read(Sensor *sensor, const SensorTag tag, void *buf, si static errno_t lsm6dso32_open(Sensor *sensor) { // Perform software reset - errno_t err = lsm6dso32_write_byte(sensor, CTRL3_C, 0x01); + errno_t err = devctl(sensor->loc.bus, DCMD_I2C_LOCK, NULL, 0, NULL); + if (err != EOK) return err; + + err = lsm6dso32_write_byte(sensor, CTRL3_C, 0x01); return_err(err); // TODO: We will want to operate in continuous mode for our use case (polling) @@ -242,8 +263,11 @@ static errno_t lsm6dso32_open(Sensor *sensor) { // TODO: what full-scale selection? Keep 500 for now ((LSM6DSO32Context *)(sensor->context.data))->gyro_fsr = 500; err = lsm6dso32_write_byte(sensor, CTRL2_G, 0xA1); + return_err(err); // TODO: what filter configuration will give the best measurements? +return_defer: + devctl(sensor->loc.bus, DCMD_I2C_UNLOCK, NULL, 0, NULL); return err; } diff --git a/src/sensors/ms5611/ms5611.c b/src/sensors/ms5611/ms5611.c index e7ff9ed..5bc6eff 100644 --- a/src/sensors/ms5611/ms5611.c +++ b/src/sensors/ms5611/ms5611.c @@ -19,6 +19,7 @@ #include #include #include +#include #include /** Number of calibration coefficients */ @@ -26,7 +27,7 @@ /** Macro to early return an error. */ #define return_err(err) \ - if (err != EOK) return err + if (err != EOK) goto return_defer /** Defines the universal gas constant. */ #define R 8.31432 @@ -91,7 +92,15 @@ static errno_t ms5611_reset(SensorLocation *loc) { uint8_t reset_cmd[sizeof(i2c_send_t) + 1]; memcpy(reset_cmd, &reset, sizeof(reset)); reset_cmd[sizeof(reset)] = CMD_RESET; - return devctl(loc->bus, DCMD_I2C_SEND, &reset_cmd, sizeof(reset_cmd), NULL); + + errno_t err = devctl(loc->bus, DCMD_I2C_LOCK, NULL, 0, NULL); // Lock I2C bus + return_err(err); + + err = devctl(loc->bus, DCMD_I2C_SEND, &reset_cmd, sizeof(reset_cmd), NULL); + +return_defer: + devctl(loc->bus, DCMD_I2C_UNLOCK, NULL, 0, NULL); // Unlock I2C bus + return err; } /** @@ -108,8 +117,16 @@ static errno_t ms5611_read_dreg(SensorLocation *loc, uint8_t dreg, uint32_t *val uint8_t conversion_cmd[sizeof(conversion) + 1]; memcpy(conversion_cmd, &conversion, sizeof(conversion)); conversion_cmd[sizeof(conversion)] = CMD_ADC_CONV + dreg; - errno_t result = devctl(loc->bus, DCMD_I2C_SEND, &conversion_cmd, sizeof(conversion_cmd), NULL); - return_err(result); + + // Lock I2C bus before sending command + errno_t err = devctl(loc->bus, DCMD_I2C_LOCK, NULL, 0, NULL); + if (err != EOK) return err; + + err = devctl(loc->bus, DCMD_I2C_SEND, &conversion_cmd, sizeof(conversion_cmd), NULL); + if (err != EOK) { + devctl(loc->bus, DCMD_I2C_UNLOCK, NULL, 0, NULL); + return err; + } // Wait for appropriate conversion time switch (dreg & 0xF) { @@ -135,15 +152,22 @@ static errno_t ms5611_read_dreg(SensorLocation *loc, uint8_t dreg, uint32_t *val uint8_t read_cmd[sizeof(read) + 3]; memcpy(read_cmd, &read, sizeof(read)); read_cmd[sizeof(read)] = CMD_ADC_READ; - result = devctl(loc->bus, DCMD_I2C_SENDRECV, &read_cmd, sizeof(read_cmd), NULL); - return_err(result); + err = devctl(loc->bus, DCMD_I2C_SENDRECV, &read_cmd, sizeof(read_cmd), NULL); + + if (err != EOK) { + devctl(loc->bus, DCMD_I2C_UNLOCK, NULL, 0, NULL); + return err; + } + + // Unlock I2C bus + err = devctl(loc->bus, DCMD_I2C_UNLOCK, NULL, 0, NULL); *value = 0; *value += read_cmd[sizeof(read)] * 65536; *value += read_cmd[sizeof(read) + 1] * 256; *value += read_cmd[sizeof(read) + 2]; - return EOK; + return err; } /** @@ -154,8 +178,8 @@ static errno_t ms5611_read_dreg(SensorLocation *loc, uint8_t dreg, uint32_t *val static errno_t ms5611_open(Sensor *sensor) { // Load calibration into PROM - errno_t op_status = ms5611_reset(&sensor->loc); - return_err(op_status); + errno_t err = ms5611_reset(&sensor->loc); + if (err != EOK) return err; usleep(10000); // Takes some time to reset // PROM read command buffer @@ -165,21 +189,27 @@ static errno_t ms5611_open(Sensor *sensor) { // Read calibration data into sensor context MS5611Context *ms5611_context = (MS5611Context *)sensor->context.data; + err = devctl(sensor->loc.bus, DCMD_I2C_LOCK, NULL, 0, NULL); // Lock I2C bus + if (err != EOK) return err; + for (uint8_t i = 0; i < NUM_COEFFICIENTS; i++) { // Read from PROM prom_read_cmd[sizeof(prom_read)] = CMD_PROM_RD + sizeof(uint16_t) * i; // Command to read next coefficient - op_status = devctl(sensor->loc.bus, DCMD_I2C_SENDRECV, &prom_read_cmd, sizeof(prom_read_cmd), NULL); - return_err(op_status); + err = devctl(sensor->loc.bus, DCMD_I2C_SENDRECV, &prom_read_cmd, sizeof(prom_read_cmd), NULL); + if (err != EOK) break; // Store calibration coefficient memcpy_be(&ms5611_context->coefs[i], &prom_read_cmd[sizeof(prom_read)], sizeof(uint16_t)); } + err = devctl(sensor->loc.bus, DCMD_I2C_UNLOCK, NULL, 0, NULL); // Unlock I2C bus + if (err != EOK) return err; + // Get the ground pressure size_t nbytes; - errno_t pressure_read = sensor->read(sensor, TAG_PRESSURE, &ms5611_context->ground_pressure, &nbytes); - return pressure_read; + err = sensor->read(sensor, TAG_PRESSURE, &ms5611_context->ground_pressure, &nbytes); + return err; } /** @@ -194,10 +224,10 @@ static errno_t ms5611_read(Sensor *sensor, const SensorTag tag, void *buf, size_ // Read D registers with configured precision uint32_t d1, d2; - errno_t dread_res = ms5611_read_dreg(&sensor->loc, D1 + PRECISIONS[sensor->precision], &d1); - return_err(dread_res); - dread_res = ms5611_read_dreg(&sensor->loc, D2 + PRECISIONS[sensor->precision], &d2); - return_err(dread_res); + errno_t err = ms5611_read_dreg(&sensor->loc, D1 + PRECISIONS[sensor->precision], &d1); + if (err != EOK) return err; + err = ms5611_read_dreg(&sensor->loc, D2 + PRECISIONS[sensor->precision], &d2); + if (err != EOK) return err; // Extract context MS5611Context *ctx = (MS5611Context *)sensor->context.data; diff --git a/src/sensors/sht41/sht41.c b/src/sensors/sht41/sht41.c index c533c1b..0b80329 100644 --- a/src/sensors/sht41/sht41.c +++ b/src/sensors/sht41/sht41.c @@ -8,12 +8,9 @@ #include "hw/i2c.h" #include #include +#include #include -/** Macro to early return an error. */ -#define return_err(err) \ - if (err != EOK) return err - /** Defines positions of most and least significant bytes in the sensor's response for temperatue */ #define SHT41_T_MSB 0 #define SHT41_T_LSB 1 @@ -116,21 +113,33 @@ static errno_t sht41_read(Sensor *sensor, const SensorTag tag, void *buf, size_t uint8_t send_cmd[sizeof(send) + 1]; memcpy(send_cmd, &send, sizeof(send)); send_cmd[sizeof(send)] = PRECISIONS[sensor->precision]; - errno_t result = devctl(sensor->loc.bus, DCMD_I2C_SEND, &send_cmd, sizeof(send_cmd), NULL); - return_err(result); + + errno_t err = devctl(sensor->loc.bus, DCMD_I2C_LOCK, NULL, 0, NULL); + if (err != EOK) return err; + + err = devctl(sensor->loc.bus, DCMD_I2C_SEND, &send_cmd, sizeof(send_cmd), NULL); + if (err != EOK) { + devctl(sensor->loc.bus, DCMD_I2C_UNLOCK, NULL, 0, NULL); + return err; + }; // Wait for the measurement to take place, depends on precision usleep(MEASUREMENT_TIMES[sensor->precision]); i2c_recv_t read = {.slave = sensor->loc.addr, .stop = 1, .len = 6}; uint8_t read_cmd[sizeof(send) + 6]; memcpy(read_cmd, &read, sizeof(read)); - result = devctl(sensor->loc.bus, DCMD_I2C_RECV, &read_cmd, sizeof(read_cmd), NULL); - return_err(result); + err = devctl(sensor->loc.bus, DCMD_I2C_RECV, &read_cmd, sizeof(read_cmd), NULL); + if (err != EOK) { + devctl(sensor->loc.bus, DCMD_I2C_UNLOCK, NULL, 0, NULL); + return err; + }; // For now, just discard one half of the measurement due to timestamping and the sensor interface switch (tag) { case TAG_TEMPERATURE: { - return_err(check_crc(read_cmd + sizeof(read) + SHT41_T_CRC, SHT41_CRC_LEN)); + err = check_crc(read_cmd + sizeof(read) + SHT41_T_CRC, SHT41_CRC_LEN); + if (err != EOK) return err; + int t_ticks = (read_cmd[sizeof(read) + SHT41_T_MSB] * 256 + read_cmd[sizeof(read) + SHT41_T_LSB]); float temp = -45 + 175 * ((float)t_ticks / 65535); // Degrees C memcpy(buf, &temp, sizeof(temp)); @@ -138,7 +147,9 @@ static errno_t sht41_read(Sensor *sensor, const SensorTag tag, void *buf, size_t break; } case TAG_HUMIDITY: { - return_err(check_crc(read_cmd + sizeof(read) + SHT41_RH_CRC, SHT41_CRC_LEN)); + err = check_crc(read_cmd + sizeof(read) + SHT41_RH_CRC, SHT41_CRC_LEN); + if (err != EOK) return err; + int rh_ticks = (read_cmd[sizeof(read) + SHT41_RH_MSB] * 256 + read_cmd[sizeof(read) + SHT41_RH_LSB]); float hum = -6 + 125 * ((float)rh_ticks / 65535); // Degrees C // Limit values of humidity, since values outside of 0-100 @@ -154,9 +165,10 @@ static errno_t sht41_read(Sensor *sensor, const SensorTag tag, void *buf, size_t break; } default: + devctl(sensor->loc.bus, DCMD_I2C_UNLOCK, NULL, 0, NULL); return EINVAL; } - return EOK; + return devctl(sensor->loc.bus, DCMD_I2C_UNLOCK, NULL, 0, NULL); } /** @@ -170,7 +182,14 @@ static errno_t sht41_reset(SensorLocation *loc) { uint8_t reset_cmd[sizeof(i2c_send_t) + 1]; memcpy(reset_cmd, &reset, sizeof(reset)); reset_cmd[sizeof(reset)] = CMD_RESET; - return devctl(loc->bus, DCMD_I2C_SEND, &reset_cmd, sizeof(reset_cmd), NULL); + + errno_t err = devctl(loc->bus, DCMD_I2C_LOCK, NULL, 0, NULL); + err = devctl(loc->bus, DCMD_I2C_SEND, &reset_cmd, sizeof(reset_cmd), NULL); + if (err != EOK) goto return_defer; + +return_defer: + devctl(loc->bus, DCMD_I2C_UNLOCK, NULL, 0, NULL); + return err; } /**