MS5525.cpp

Go to the documentation of this file.

/****************************************************************************
 *
 *   Copyright (c) 2017 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

#include "MS5525.hpp"

int
MS5525::measure()
{
    int ret = PX4_ERROR;

    if (_inited) {
        // send the command to begin a conversion.
        uint8_t cmd = _current_cmd;
        ret = transfer(&cmd, 1, nullptr, 0);

        if (ret != PX4_OK) {
            perf_count(_comms_errors);
        }

    } else {
        _inited = init_ms5525();

        if (_inited) {
            ret = PX4_OK;
        }
    }

    return ret;
}

bool
MS5525::init_ms5525()
{
    // Step 1 - reset
    uint8_t cmd = CMD_RESET;
    int ret = transfer(&cmd, 1, nullptr, 0);

    if (ret != PX4_OK) {
        perf_count(_comms_errors);
        return false;
    }

    px4_usleep(3000);

    // Step 2 - read calibration coefficients from prom

    // prom layout
    // 0 factory data and the setup
    // 1-6 calibration coefficients
    // 7 serial code and CRC
    uint16_t prom[8];

    for (uint8_t i = 0; i < 8; i++) {
        cmd = CMD_PROM_START + i * 2;

        // request PROM value
        ret = transfer(&cmd, 1, nullptr, 0);

        if (ret != PX4_OK) {
            perf_count(_comms_errors);
            return false;
        }

        // read 2 byte value
        uint8_t val[2];
        ret = transfer(nullptr, 0, &val[0], 2);

        if (ret == PX4_OK) {
            prom[i] = (val[0] << 8) | val[1];

        } else {
            perf_count(_comms_errors);
            return false;
        }
    }

    // Step 3 - check CRC
    const uint8_t crc = prom_crc4(prom);
    const uint8_t onboard_crc = prom[7] & 0xF;

    if (crc == onboard_crc) {
        // store valid calibration coefficients
        C1 = prom[1];
        C2 = prom[2];
        C3 = prom[3];
        C4 = prom[4];
        C5 = prom[5];
        C6 = prom[6];

        Tref = int64_t(C5) * (1UL << Q5);
        _device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_MS5525;

        return true;

    } else {
        PX4_ERR("CRC mismatch");
        return false;
    }
}

uint8_t
MS5525::prom_crc4(uint16_t n_prom[]) const
{
    // see Measurement Specialties AN520

    // crc remainder
    unsigned int n_rem = 0x00;

    // original value of the crc
    unsigned int crc_read = n_prom[7]; // save read CRC
    n_prom[7] = (0xFF00 & (n_prom[7])); // CRC byte is replaced by 0

    // operation is performed on bytes
    for (int cnt = 0; cnt < 16; cnt++) {
        // choose LSB or MSB
        if (cnt % 2 == 1) {
            n_rem ^= (unsigned short)((n_prom[cnt >> 1]) & 0x00FF);

        } else {
            n_rem ^= (unsigned short)(n_prom[cnt >> 1] >> 8);
        }

        for (uint8_t n_bit = 8; n_bit > 0; n_bit--) {
            if (n_rem & (0x8000)) {
                n_rem = (n_rem << 1) ^ 0x3000;

            } else {
                n_rem = (n_rem << 1);
            }
        }
    }

    n_rem = (0x000F & (n_rem >> 12)); // final 4-bit reminder is CRC code
    n_prom[7] = crc_read; // restore the crc_read to its original place

    return (n_rem ^ 0x00);
}

int
MS5525::collect()
{
    perf_begin(_sample_perf);

    // read ADC
    uint8_t cmd = CMD_ADC_READ;
    int ret = transfer(&cmd, 1, nullptr, 0);

    if (ret != PX4_OK) {
        perf_count(_comms_errors);
        return ret;
    }

    // read 24 bits from the sensor
    uint8_t val[3];
    ret = transfer(nullptr, 0, &val[0], 3);

    if (ret != PX4_OK) {
        perf_count(_comms_errors);
        return ret;
    }

    uint32_t adc = (val[0] << 16) | (val[1] << 8) | val[2];

    // If the conversion is not executed before the ADC read command, or the ADC read command is repeated, it will give 0 as the output
    // result. If the ADC read command is sent during conversion the result will be 0, the conversion will not stop and
    // the final result will be wrong. Conversion sequence sent during the already started conversion process will yield
    // incorrect result as well.
    if (adc == 0) {
        perf_count(_comms_errors);
        return EAGAIN;
    }

    if (_current_cmd == CMD_CONVERT_PRES) {
        D1 = adc;
        _pressure_count++;

        if (_pressure_count > 9) {
            _pressure_count = 0;
            _current_cmd = CMD_CONVERT_TEMP;
        }

    } else if (_current_cmd == CMD_CONVERT_TEMP) {
        D2 = adc;
        _current_cmd = CMD_CONVERT_PRES;

        // only calculate and publish after new pressure readings
        return PX4_OK;
    }

    // not ready yet
    if (D1 == 0 || D2 == 0) {
        return EAGAIN;
    }

    // Difference between actual and reference temperature
    //  dT = D2 - Tref
    const int64_t dT = D2 - Tref;

    // Measured temperature
    //  TEMP = 20°C + dT * TEMPSENS
    const int64_t TEMP = 2000 + (dT * int64_t(C6)) / (1UL << Q6);

    // Offset at actual temperature
    //  OFF = OFF_T1 + TCO * dT
    const int64_t OFF = int64_t(C2) * (1UL << Q2) + (int64_t(C4) * dT) / (1UL << Q4);

    // Sensitivity at actual temperature
    //  SENS = SENS_T1 + TCS * dT
    const int64_t SENS = int64_t(C1) * (1UL << Q1) + (int64_t(C3) * dT) / (1UL << Q3);

    // Temperature Compensated Pressure (example 24996 = 2.4996 psi)
    //  P = D1 * SENS - OFF
    const int64_t P = (D1 * SENS / (1UL << 21) - OFF) / (1UL << 15);

    const float diff_press_PSI = P * 0.0001f;

    // 1 PSI = 6894.76 Pascals
    static constexpr float PSI_to_Pa = 6894.757f;
    const float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa;

    const float temperature_c = TEMP * 0.01f;

    differential_pressure_s diff_pressure = {
        .timestamp = hrt_absolute_time(),
        .error_count = perf_event_count(_comms_errors),
        .differential_pressure_raw_pa = diff_press_pa_raw - _diff_pres_offset,
        .differential_pressure_filtered_pa =  _filter.apply(diff_press_pa_raw) - _diff_pres_offset,
        .temperature = temperature_c,
        .device_id = _device_id.devid
    };

    if (_airspeed_pub != nullptr && !(_pub_blocked)) {
        /* publish it */
        orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &diff_pressure);
    }

    ret = OK;

    perf_end(_sample_perf);

    return ret;
}

void
MS5525::Run()
{
    int ret = PX4_ERROR;

    // collection phase
    if (_collect_phase) {
        // perform collection
        ret = collect();

        if (OK != ret) {
            /* restart the measurement state machine */
            start();
            _sensor_ok = false;
            return;
        }

        // next phase is measurement
        _collect_phase = false;

        // is there a collect->measure gap?
        if (_measure_interval > CONVERSION_INTERVAL) {

            // schedule a fresh cycle call when we are ready to measure again
            ScheduleDelayed(_measure_interval - CONVERSION_INTERVAL);

            return;
        }
    }

    /* measurement phase */
    ret = measure();

    if (OK != ret) {
        DEVICE_DEBUG("measure error");
    }

    _sensor_ok = (ret == OK);

    // next phase is collection
    _collect_phase = true;

    // schedule a fresh cycle call when the measurement is done
    ScheduleDelayed(CONVERSION_INTERVAL);
}