| /* |
| * Licensed to the Apache Software Foundation (ASF) under one |
| * or more contributor license agreements. See the NOTICE file |
| * distributed with this work for additional information |
| * regarding copyright ownership. The ASF licenses this file |
| * to you under the Apache License, Version 2.0 (the |
| * "License"); you may not use this file except in compliance |
| * with the License. You may obtain a copy of the License at |
| * |
| * http://www.apache.org/licenses/LICENSE-2.0 |
| * |
| * Unless required by applicable law or agreed to in writing, |
| * software distributed under the License is distributed on an |
| * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY |
| * KIND, either express or implied. See the License for the |
| * specific language governing permissions and limitations |
| * under the License. |
| */ |
| |
| #include <assert.h> |
| #include <stdio.h> |
| #include <errno.h> |
| #include <string.h> |
| |
| #include "defs/error.h" |
| #include "os/os.h" |
| #include "sysinit/sysinit.h" |
| #include "hal/hal_i2c.h" |
| #include "i2cn/i2cn.h" |
| #include "sensor/sensor.h" |
| #include "ms5840/ms5840.h" |
| #include "sensor/temperature.h" |
| #include "sensor/pressure.h" |
| #include "ms5840_priv.h" |
| #include "os/os_cputime.h" |
| #include "console/console.h" |
| #include "modlog/modlog.h" |
| #include "stats/stats.h" |
| #include <syscfg/syscfg.h> |
| |
| static uint16_t cnv_time[6] = { |
| MS5840_CNV_TIME_OSR_256, |
| MS5840_CNV_TIME_OSR_512, |
| MS5840_CNV_TIME_OSR_1024, |
| MS5840_CNV_TIME_OSR_2048, |
| MS5840_CNV_TIME_OSR_4096, |
| MS5840_CNV_TIME_OSR_8192 |
| }; |
| |
| /* Define the stats section and records */ |
| STATS_SECT_START(ms5840_stat_section) |
| STATS_SECT_ENTRY(read_errors) |
| STATS_SECT_ENTRY(write_errors) |
| STATS_SECT_ENTRY(eeprom_crc_errors) |
| STATS_SECT_END |
| |
| /* Define stat names for querying */ |
| STATS_NAME_START(ms5840_stat_section) |
| STATS_NAME(ms5840_stat_section, read_errors) |
| STATS_NAME(ms5840_stat_section, write_errors) |
| STATS_NAME(ms5840_stat_section, eeprom_crc_errors) |
| STATS_NAME_END(ms5840_stat_section) |
| |
| /* Global variable used to hold stats data */ |
| STATS_SECT_DECL(ms5840_stat_section) g_ms5840stats; |
| |
| #define MS5840_LOG(lvl_, ...) \ |
| MODLOG_ ## lvl_(MYNEWT_VAL(MS5840_LOG_MODULE), __VA_ARGS__) |
| |
| /* Exports for the sensor API */ |
| static int ms5840_sensor_read(struct sensor *, sensor_type_t, |
| sensor_data_func_t, void *, uint32_t); |
| static int ms5840_sensor_get_config(struct sensor *, sensor_type_t, |
| struct sensor_cfg *); |
| static int ms5840_sensor_set_config(struct sensor *, void *); |
| |
| static const struct sensor_driver g_ms5840_sensor_driver = { |
| .sd_read = ms5840_sensor_read, |
| .sd_get_config = ms5840_sensor_get_config, |
| .sd_set_config = ms5840_sensor_set_config, |
| }; |
| |
| /** |
| * Expects to be called back through os_dev_create(). |
| * |
| * @param The device object associated with ms5840 |
| * @param Argument passed to OS device init, unused |
| * |
| * @return 0 on success, non-zero error on failure. |
| */ |
| int |
| ms5840_init(struct os_dev *dev, void *arg) |
| { |
| struct ms5840 *ms5840; |
| struct sensor *sensor; |
| struct sensor_itf *itf; |
| int rc; |
| |
| if (!arg || !dev) { |
| rc = SYS_ENODEV; |
| goto err; |
| } |
| |
| ms5840 = (struct ms5840 *)dev; |
| |
| sensor = &ms5840->sensor; |
| |
| itf = SENSOR_GET_ITF(sensor); |
| |
| /* Initialise the stats entry */ |
| rc = stats_init( |
| STATS_HDR(g_ms5840stats), |
| STATS_SIZE_INIT_PARMS(g_ms5840stats, STATS_SIZE_32), |
| STATS_NAME_INIT_PARMS(ms5840_stat_section)); |
| SYSINIT_PANIC_ASSERT(rc == 0); |
| /* Register the entry with the stats registry */ |
| rc = stats_register(dev->od_name, STATS_HDR(g_ms5840stats)); |
| SYSINIT_PANIC_ASSERT(rc == 0); |
| |
| rc = sensor_init(sensor, dev); |
| if (rc != 0) { |
| goto err; |
| } |
| |
| /* Add the driver with all the supported type */ |
| rc = sensor_set_driver(sensor, SENSOR_TYPE_AMBIENT_TEMPERATURE | |
| SENSOR_TYPE_PRESSURE, |
| (struct sensor_driver *)&g_ms5840_sensor_driver); |
| if (rc) { |
| goto err; |
| } |
| |
| /* Set the interface */ |
| rc = sensor_set_interface(sensor, arg); |
| if (rc) { |
| goto err; |
| } |
| |
| rc = sensor_mgr_register(sensor); |
| if (rc) { |
| goto err; |
| } |
| |
| rc = ms5840_read_eeprom(itf, ms5840->pdd.eeprom_coeff); |
| if (rc) { |
| goto err; |
| } |
| |
| return 0; |
| err: |
| return rc; |
| |
| } |
| |
| static int |
| ms5840_sensor_read(struct sensor *sensor, sensor_type_t type, |
| sensor_data_func_t data_func, void *data_arg, uint32_t timeout) |
| { |
| uint32_t rawtemp; |
| uint32_t rawpress; |
| int32_t comptemp; |
| int32_t deltat; |
| float temperature; |
| float pressure; |
| struct sensor_itf *itf; |
| struct ms5840 *ms5840; |
| struct ms5840_cfg *cfg; |
| |
| int rc; |
| union { |
| struct sensor_temp_data std; |
| struct sensor_press_data spd; |
| } databuf; |
| |
| if (!(type & SENSOR_TYPE_PRESSURE) && |
| !(type & SENSOR_TYPE_AMBIENT_TEMPERATURE)) { |
| rc = SYS_EINVAL; |
| goto err; |
| } |
| |
| itf = SENSOR_GET_ITF(sensor); |
| |
| ms5840 = (struct ms5840 *)SENSOR_GET_DEVICE(sensor); |
| |
| cfg = &(ms5840->cfg); |
| |
| temperature = pressure = 0; |
| |
| /* Get a new pressure sample */ |
| if (type & SENSOR_TYPE_PRESSURE) { |
| rc = ms5840_get_rawtemp(itf, &rawtemp, cfg->mc_s_temp_res_osr); |
| if (rc) { |
| goto err; |
| } |
| |
| rc = ms5840_get_rawpress(itf, &rawpress, cfg->mc_s_press_res_osr); |
| if (rc) { |
| goto err; |
| } |
| |
| /* compensate using temperature and pressure coefficients |
| * competemp is the first order compensated temperature |
| * which is used as input to the pressure compensation |
| */ |
| temperature = ms5840_compensate_temperature(ms5840->pdd.eeprom_coeff, rawtemp, |
| &comptemp, &deltat); |
| pressure = ms5840_compensate_pressure(ms5840->pdd.eeprom_coeff, comptemp, |
| rawpress, deltat); |
| |
| databuf.spd.spd_press = pressure; |
| databuf.spd.spd_press_is_valid = 1; |
| |
| /* Call data function */ |
| rc = data_func(sensor, data_arg, &databuf.spd, SENSOR_TYPE_PRESSURE); |
| if (rc) { |
| goto err; |
| } |
| } |
| |
| /* Get a new temperature sample */ |
| if (type & SENSOR_TYPE_AMBIENT_TEMPERATURE) { |
| if (!temperature) { |
| rc = ms5840_get_rawtemp(itf, &rawtemp, cfg->mc_s_temp_res_osr); |
| if (rc) { |
| goto err; |
| } |
| |
| temperature = ms5840_compensate_temperature(ms5840->pdd.eeprom_coeff, rawtemp, |
| NULL, NULL); |
| } |
| |
| databuf.std.std_temp = temperature; |
| databuf.std.std_temp_is_valid = 1; |
| |
| /* Call data function */ |
| rc = data_func(sensor, data_arg, &databuf.std, |
| SENSOR_TYPE_AMBIENT_TEMPERATURE); |
| if (rc) { |
| goto err; |
| } |
| } |
| |
| return 0; |
| err: |
| return rc; |
| } |
| |
| static int |
| ms5840_sensor_get_config(struct sensor *sensor, sensor_type_t type, |
| struct sensor_cfg *cfg) |
| { |
| int rc; |
| |
| if (!(type & SENSOR_TYPE_PRESSURE) || |
| !(type & SENSOR_TYPE_AMBIENT_TEMPERATURE)) { |
| rc = SYS_EINVAL; |
| goto err; |
| } |
| |
| cfg->sc_valtype = SENSOR_VALUE_TYPE_FLOAT; |
| |
| return (0); |
| err: |
| return (rc); |
| } |
| |
| static int |
| ms5840_sensor_set_config(struct sensor *sensor, void *cfg) |
| { |
| struct ms5840* ms5840 = (struct ms5840 *)SENSOR_GET_DEVICE(sensor); |
| |
| return ms5840_config(ms5840, (struct ms5840_cfg*)cfg); |
| } |
| |
| /** |
| * Configure MS5840 sensor |
| * |
| * @param Sensor device MS5840 structure |
| * @param Sensor device MS5840 config |
| * |
| * @return 0 on success, non-zero on failure |
| */ |
| int |
| ms5840_config(struct ms5840 *ms5840, struct ms5840_cfg *cfg) |
| { |
| int rc; |
| struct sensor_itf *itf; |
| |
| itf = SENSOR_GET_ITF(&(ms5840->sensor)); |
| |
| rc = ms5840_reset(itf); |
| if (rc) { |
| goto err; |
| } |
| |
| rc = sensor_set_type_mask(&(ms5840->sensor), cfg->mc_s_mask); |
| if (rc) { |
| goto err; |
| } |
| |
| ms5840->cfg.mc_s_temp_res_osr = cfg->mc_s_temp_res_osr; |
| |
| ms5840->cfg.mc_s_press_res_osr = cfg->mc_s_press_res_osr; |
| |
| ms5840->cfg.mc_s_mask = cfg->mc_s_mask; |
| |
| return 0; |
| err: |
| return (rc); |
| } |
| |
| /** |
| * Write multiple length data to MS5840 sensor over I2C |
| * |
| * @param The sensor interface |
| * @param register address |
| * @param variable length payload |
| * @param length of the payload to write |
| * |
| * @return 0 on success, non-zero on failure |
| */ |
| int |
| ms5840_writelen(struct sensor_itf *itf, uint8_t addr, uint8_t *buffer, |
| uint8_t len) |
| { |
| int rc; |
| |
| struct hal_i2c_master_data data_struct = { |
| .address = itf->si_addr, |
| .len = 1, |
| .buffer = &addr |
| }; |
| |
| rc = sensor_itf_lock(itf, MYNEWT_VAL(MS5840_ITF_LOCK_TMO)); |
| if (rc) { |
| return rc; |
| } |
| |
| /* Register write */ |
| rc = i2cn_master_write(itf->si_num, &data_struct, MYNEWT_VAL(MS5840_I2C_TIMEOUT_TICKS), 1, |
| MYNEWT_VAL(MS5840_I2C_RETRIES)); |
| if (rc) { |
| MS5840_LOG(ERROR, "I2C write command write failed at address 0x%02X\n", |
| data_struct.address); |
| STATS_INC(g_ms5840stats, write_errors); |
| } |
| |
| sensor_itf_unlock(itf); |
| |
| return rc; |
| } |
| |
| /** |
| * Read multiple length data from MS5840 sensor over I2C |
| * |
| * @param The sensor interface |
| * @param register address |
| * @param variable length buffer |
| * @param length of the payload to read |
| * |
| * @return 0 on success, non-zero on failure |
| */ |
| int |
| ms5840_readlen(struct sensor_itf *itf, uint8_t addr, uint8_t *buffer, |
| uint8_t len) |
| { |
| int rc; |
| uint8_t payload[3] = {addr, 0, 0}; |
| |
| struct hal_i2c_master_data data_struct = { |
| .address = itf->si_addr, |
| .len = 1, |
| .buffer = payload |
| }; |
| |
| /* Clear the supplied buffer */ |
| memset(buffer, 0, len); |
| |
| rc = sensor_itf_lock(itf, MYNEWT_VAL(MS5840_ITF_LOCK_TMO)); |
| if (rc) { |
| return rc; |
| } |
| |
| /* Command write */ |
| rc = i2cn_master_write(itf->si_num, &data_struct, MYNEWT_VAL(MS5840_I2C_TIMEOUT_TICKS), 1, |
| MYNEWT_VAL(MS5840_I2C_RETRIES)); |
| if (rc) { |
| MS5840_LOG(ERROR, "I2C read command write failed at address 0x%02X\n", |
| data_struct.address); |
| STATS_INC(g_ms5840stats, write_errors); |
| goto err; |
| } |
| |
| /* Read len bytes back */ |
| memset(payload, 0, sizeof(payload)); |
| data_struct.len = len; |
| rc = i2cn_master_read(itf->si_num, &data_struct, MYNEWT_VAL(MS5840_I2C_TIMEOUT_TICKS), 1, |
| MYNEWT_VAL(MS5840_I2C_RETRIES)); |
| if (rc) { |
| MS5840_LOG(ERROR, "Failed to read from 0x%02X:0x%02X\n", |
| data_struct.address, addr); |
| STATS_INC(g_ms5840stats, read_errors); |
| goto err; |
| } |
| |
| /* Copy the I2C results into the supplied buffer */ |
| memcpy(buffer, payload, len); |
| |
| err: |
| sensor_itf_unlock(itf); |
| return rc; |
| } |
| |
| /** |
| * Reads the ms5840 EEPROM coefficients for computation and |
| * does a CRC check on them |
| * |
| * @param the sensor interface |
| * @param buffer to fill up the coefficients |
| * |
| * @return 0 on success, non-zero on failure |
| */ |
| int |
| ms5840_read_eeprom(struct sensor_itf *itf, uint16_t *coeff) |
| { |
| int idx; |
| int rc; |
| uint16_t payload[MS5840_NUMBER_COEFFS]; |
| |
| for(idx = 0; idx < MS5840_NUMBER_COEFFS; idx++) { |
| rc = ms5840_readlen(itf, MS5840_CMD_PROM_READ_ADDR0 + idx * 2, |
| (uint8_t *)(payload + idx), 2); |
| if (rc) { |
| goto err; |
| } |
| |
| payload[idx] = (((payload[idx] & 0xFF00) >> 8)| |
| ((payload[idx] & 0x00FF) << 8)); |
| } |
| |
| rc = ms5840_crc_check(payload, (payload[MS5840_IDX_CRC] & 0xF000) >> 12); |
| if (rc) { |
| rc = SYS_EINVAL; |
| MS5840_LOG(ERROR, "Failure in CRC, 0x%02X\n", |
| payload[MS5840_IDX_CRC] & 0xF000 >> 12); |
| STATS_INC(g_ms5840stats, eeprom_crc_errors); |
| goto err; |
| } |
| |
| memcpy(coeff, payload, sizeof(payload)); |
| |
| return 0; |
| err: |
| return rc; |
| } |
| |
| /** |
| * Compensate for pressure using coefficients from the EEPROM |
| * |
| * @param ptr to coefficients |
| * @param first order compensated temperature |
| * @param raw pressure |
| * @param deltat temperature |
| * |
| * @return second order temperature compensated pressure |
| */ |
| float |
| ms5840_compensate_pressure(uint16_t *coeffs, int32_t temp, |
| uint32_t rawpress, int32_t deltat) |
| { |
| int64_t off, sens, off2, sens2; |
| |
| off2 = sens2 = 0; |
| |
| /* off = off_T1 + TCO * dt */ |
| off = ((int64_t)(coeffs[MS5840_IDX_PRESS_OFF]) << 17) + |
| (((int64_t)(coeffs[MS5840_IDX_TEMP_COEFF_PRESS_OFF]) * deltat) >> 6); |
| |
| /* sensitivity at actual temperature = sens_T1 + TCS * dt */ |
| sens = ((int64_t)coeffs[MS5840_IDX_PRESS_SENS] << 16) + |
| (((int64_t)coeffs[MS5840_IDX_TEMP_COEFF_PRESS_SENS] * deltat) >> 7); |
| |
| /* second order temperature compensation */ |
| if(temp < 1000) { |
| /* low temperature */ |
| off2 = (35 * ((int64_t)temp - 2000) * ((int64_t)temp - 2000)) >> 3; |
| sens2 = (63 * ((int64_t)temp - 2000) * ((int64_t)temp - 2000)) >> 5; |
| } else if (temp < 2000) { |
| off2 = (30 * ((int64_t)temp - 2000) * ((int64_t)temp - 2000)) >> 8; |
| } |
| |
| off2 = off - off2; |
| |
| sens2 = sens - sens2; |
| |
| /* temperature compensated second order pressure = D1 * sens - off */ |
| return ((float)(((rawpress * sens2) >> 21) - off2)/32768); |
| } |
| |
| /** |
| * Compensate for temperature using coefficients from the EEPROM |
| * |
| * @param ptr to coefficients |
| * @param compensated temperature |
| * @param raw temperature |
| * @param optional ptr to fill up first order compensated temperature |
| * @param optional ptr to fill up delta temperature |
| * |
| * @return second order temperature compensated temperature |
| */ |
| float |
| ms5840_compensate_temperature(uint16_t *coeffs, uint32_t rawtemp, |
| int32_t *comptemp, int32_t *deltat) |
| { |
| int32_t dt, temp; |
| int64_t t2; |
| |
| t2 = 0; |
| |
| /* difference between actual and reference temperature = D2 - Tref */ |
| dt = (int32_t)rawtemp - ((int32_t)coeffs[MS5840_IDX_REF_TEMP] << 8); |
| |
| /* actual temperature = 2000 + dt * tempsens */ |
| temp = 2000 + ((int64_t)((int64_t)dt * (int64_t)coeffs[MS5840_IDX_TEMP_COEFF_TEMP]) >> 23); |
| |
| if (comptemp) { |
| *comptemp = temp; |
| } |
| |
| if (deltat) { |
| *deltat = dt; |
| } |
| |
| if(temp < 1000) { |
| /* low temperature */ |
| t2 = (11 * (int64_t)dt * (int64_t)dt) >> 35; |
| } |
| |
| /* second order temperature */ |
| return (((float)temp - t2)/100); |
| } |
| |
| /** |
| * Triggers conversion and reads ADC value |
| * |
| * @param the sensor interface |
| * @param cmd used for conversion, considers temperature, pressure and OSR |
| * @param ptr to ADC value |
| * |
| * @return 0 on success, non-zero on failure |
| */ |
| static int |
| ms5840_get_raw_data(struct sensor_itf *itf, uint8_t cmd, uint32_t *data) |
| { |
| int rc; |
| uint8_t payload[3] = {0}; |
| |
| /* send conversion command based on OSR, temperature and pressure */ |
| rc = ms5840_writelen(itf, cmd, payload, 0); |
| if (rc) { |
| goto err; |
| } |
| |
| /* delay conversion depending on resolution */ |
| os_cputime_delay_usecs(cnv_time[(cmd & MS5840_CNV_OSR_MASK)/2]); |
| |
| /* read adc value */ |
| rc = ms5840_readlen(itf, MS5840_CMD_ADC_READ, payload, 3); |
| if (rc) { |
| goto err; |
| } |
| |
| *data = ((uint32_t)payload[0] << 16) | ((uint32_t)payload[1] << 8) | payload[2]; |
| |
| return 0; |
| err: |
| return rc; |
| } |
| |
| /** |
| * Reads the temperature ADC value |
| * |
| * @param the sensor interface |
| * @param raw adc temperature value |
| * @param resolution osr |
| * |
| * @return 0 on success, non-zero on failure |
| */ |
| int |
| ms5840_get_rawtemp(struct sensor_itf *itf, uint32_t *rawtemp, |
| uint8_t res_osr) |
| { |
| uint8_t cmd; |
| uint32_t tmp; |
| int rc; |
| |
| /* read temperature ADC value */ |
| cmd = res_osr | MS5840_CMD_TEMP; |
| rc = ms5840_get_raw_data(itf, cmd, &tmp); |
| if (rc) { |
| goto err; |
| } |
| |
| *rawtemp = tmp; |
| |
| return 0; |
| err: |
| return rc; |
| } |
| |
| /** |
| * Reads the pressure ADC value |
| * |
| * @param the sensor interface |
| * @param raw adc pressure value |
| * @param resolution osr |
| * |
| * @return 0 on success, non-zero on failure |
| */ |
| int |
| ms5840_get_rawpress(struct sensor_itf *itf, uint32_t *rawpress, |
| uint8_t res_osr) |
| { |
| uint8_t cmd; |
| uint32_t tmp; |
| int rc; |
| |
| /* read pressure ADC value */ |
| cmd = res_osr | MS5840_CMD_PRESS; |
| rc = ms5840_get_raw_data(itf, cmd, &tmp); |
| if (rc) { |
| goto err; |
| } |
| |
| *rawpress = tmp; |
| |
| return 0; |
| err: |
| return rc; |
| } |
| |
| /** |
| * Resets the MS5840 chip |
| * |
| * @param the sensor interface |
| * |
| * @return 0 on success, non-zero on failure |
| */ |
| int |
| ms5840_reset(struct sensor_itf *itf) |
| { |
| uint8_t txdata; |
| |
| txdata = 0; |
| |
| return ms5840_writelen(itf, MS5840_CMD_RESET, &txdata, 0); |
| } |
| |
| /** |
| * crc4 check for MS5840 EEPROM |
| * |
| * @param buffer containing EEPROM coefficients |
| * @param crc to compare with |
| * |
| * return 0 on success (CRC is OK), non-zero on failure |
| */ |
| int |
| ms5840_crc_check(uint16_t *prom, uint8_t crc) |
| { |
| uint8_t cnt, bit; |
| uint16_t rem, crc_read; |
| |
| rem = 0x00; |
| crc_read = prom[0]; |
| prom[MS5840_NUMBER_COEFFS] = 0; |
| |
| /* Clear the CRC byte */ |
| prom[0] = (0x0FFF & prom[0]); |
| |
| for(cnt = 0; cnt < (MS5840_NUMBER_COEFFS + 1) * 2; cnt++) { |
| /* Get next byte */ |
| if (cnt%2 == 1) { |
| rem ^= (prom[cnt>>1] & 0x00FF); |
| } else { |
| rem ^= (prom[cnt>>1] >> 8); |
| } |
| |
| for(bit = 8; bit > 0; bit--) { |
| if(rem & 0x8000) { |
| rem = (rem << 1) ^ 0x3000; |
| } else { |
| rem <<= 1; |
| } |
| } |
| } |
| |
| rem >>= 12; |
| prom[0] = crc_read; |
| |
| return (rem != crc); |
| } |