airspeed_ms45xx_i2c.c

Go to the documentation of this file.

/*
 * Copyright (C) 2014 Freek van Tienen <freek.v.tienen@gmail.com>
 *               2014 Felix Ruess <felix.ruess@gmail.com>
 *
 * This file is part of paparazzi
 *
 * paparazzi is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2, or (at your option)
 * any later version.
 *
 * paparazzi is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with paparazzi; see the file COPYING.  If not, write to
 * the Free Software Foundation, 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA.
 */
 
#include "std.h"
#include "mcu_periph/i2c.h"
#include "modules/sensors/airspeed_ms45xx_i2c.h"
#include "filters/low_pass_filter.h"
#include "modules/core/abi.h"
 
#include "mcu_periph/uart.h"
#include "pprzlink/messages.h"
#include "modules/datalink/downlink.h"
 
#if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h"
#endif
 
/* Enable ABI sending */
#ifndef AIRSPEED_MS45XX_SEND_ABI
#define AIRSPEED_MS45XX_SEND_ABI true
#endif
 
#ifndef MS45XX_I2C_DEV
#define MS45XX_I2C_DEV i2c2
#endif
 
#ifndef MS45XX_I2C_ADDR
#define MS45XX_I2C_ADDR 0x50
#endif
 
#ifndef MS45XX_PRESSURE_OUTPUT_TYPE_InH2O
#define MS45XX_PRESSURE_OUTPUT_TYPE_InH2O 0
#endif
 
#ifndef MS45XX_PRESSURE_RANGE
#define MS45XX_PRESSURE_RANGE 1
#endif
 
#ifndef MS45XX_PRESSURE_TYPE
#define MS45XX_PRESSURE_TYPE 0
#endif
 
#ifndef USE_AIRSPEED_LOWPASS_FILTER
#define USE_AIRSPEED_LOWPASS_FILTER TRUE
#endif
 
#ifndef MS45XX_OUTPUT_TYPE
#define MS45XX_OUTPUT_TYPE 0
#endif
 
#define InH2O_TO_PA 249.08891
 
#define PSI_TO_PA 6894.75729
 
#if MS45XX_PRESSURE_OUTPUT_TYPE_InH2O
#define OutputPressureToPa InH2O_TO_PA
#else
#define OutputPressureToPa PSI_TO_PA
#endif
 
#if MS45XX_OUTPUT_TYPE == 0
/* Offset and scaling for OUTPUT TYPE A:
 * p_raw = (0.8*16383)/ (Pmax - Pmin) * (pressure - Pmin) + 0.1*16383
 * For differential sensors Pmax = MS45XX_PRESSURE_RANGE = -Pmin.
 *
 * p_diff = (p_raw - 0.1*16383) * 2*RANGE/(0.8*16383) - RANGE
 * p_diff = p_raw * 2*RANGE/(0.8*16383) - (RANGE + (0.1 * 16383) * 2*RANGE/(0.8*16383)
 * p_diff = p_raw * 2*RANGE/(0.8*16383) - (1.25 * RANGE)
 * p_diff = (p_raw - offset) * scale
 * then convert to Pascal
 */
#ifndef MS45XX_PRESSURE_SCALE
#define MS45XX_PRESSURE_SCALE (2 * MS45XX_PRESSURE_RANGE / (0.8 * 16383) * OutputPressureToPa)
#endif
#ifndef MS45XX_PRESSURE_OFFSET
#define MS45XX_PRESSURE_OFFSET (16383 / 2)
#endif
#else /* Can still be improved using another if statment with MS45XX_PRESSURE_TYPE etc. */
/* Offset and scaling for OUTPUT TYPE B:
 * p_raw = (0.9*16383)/ (Pmax - Pmin) * (pressure - Pmin) + 0.05*16383
 */
#ifndef MS45XX_PRESSURE_SCALE
#define MS45XX_PRESSURE_SCALE (MS45XX_PRESSURE_RANGE/(0.9*16383)*OutputPressureToPa)
#endif
#ifndef MS45XX_PRESSURE_OFFSET
#define MS45XX_PRESSURE_OFFSET (0.05*16383)
#endif
#endif
 
PRINT_CONFIG_VAR(MS45XX_OUTPUT_TYPE)
PRINT_CONFIG_VAR(MS45XX_PRESSURE_TYPE)
PRINT_CONFIG_VAR(MS45XX_PRESSURE_RANGE)
PRINT_CONFIG_VAR(MS45XX_PRESSURE_SCALE)
PRINT_CONFIG_VAR(MS45XX_PRESSURE_OFFSET)
 
 
 
#ifdef MS45XX_AIRSPEED_SCALE
PRINT_CONFIG_MSG("MS45XX changed air density. PS: Use MS45XX_PRESSURE_SCALE to calibrate the MS45XX.");
#endif
 
#define MS45XX_RHO_DIV_2 1.6327
 
 
#ifndef MS45XX_LOWPASS_TAU
#define MS45XX_LOWPASS_TAU 0.15
#endif
 
struct AirspeedMs45xx ms45xx;
static struct i2c_transaction ms45xx_trans;
#ifdef USE_AIRSPEED_LOWPASS_FILTER
static Butterworth2LowPass ms45xx_filter;
#endif
 
#if PREFLIGHT_CHECKS
/* Preflight checks */
#include "modules/checks/preflight_checks.h"
static struct preflight_check_t ms45xx_i2c_pfc;
 
static void ms45xx_preflight(struct preflight_result_t *result) {
  if(ms45xx.offset_set) {
    preflight_success(result, "Airspeed sensor succesfully nulled (MS45XX)");
  } else {
    preflight_error(result, "Airspeed sensor not nulled (MS45XX)");
  }
}
#endif // PREFLIGHT_CHECKS
 
static void ms45xx_downlink(struct transport_tx *trans, struct link_device *dev)
{
  uint8_t dev_id = MS45XX_SENDER_ID;
  float temp = ((float)ms45xx.temperature) * 0.1f;
  pprz_msg_send_AIRSPEED_RAW(trans,dev,AC_ID,
                                &dev_id,
                                &ms45xx.raw_p,
                                &ms45xx.pressure_offset,
                                &ms45xx.pressure,
                                &temp,
                                &ms45xx.airspeed);
}
 
void ms45xx_i2c_init(void)
{
  ms45xx.pressure = 0.;
  ms45xx.temperature = 0;
  ms45xx.airspeed = 0.;
  ms45xx.raw_p = 0;
  ms45xx.pressure_type = MS45XX_PRESSURE_TYPE;
  ms45xx.pressure_scale = MS45XX_PRESSURE_SCALE;
  ms45xx.pressure_offset = MS45XX_PRESSURE_OFFSET;
  ms45xx.lowpass_tau = MS45XX_LOWPASS_TAU;
  ms45xx.offset_set = false;
 
  ms45xx_trans.status = I2CTransDone;
  // setup low pass filter with time constant and 100Hz sampling freq
#ifdef USE_AIRSPEED_LOWPASS_FILTER
  init_butterworth_2_low_pass(&ms45xx_filter, ms45xx.lowpass_tau,
                              MS45XX_I2C_PERIODIC_PERIOD, 0);
#endif
 
#if PERIODIC_TELEMETRY
  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AIRSPEED_RAW, ms45xx_downlink);
#endif
 
  /* Register preflight checks */
#if PREFLIGHT_CHECKS
  preflight_check_register(&ms45xx_i2c_pfc, ms45xx_preflight);
#endif
}
 
 
void airspeed_ms45xx_i2c_change_tau(float new_tau)
{
  ms45xx.lowpass_tau = new_tau;
#ifdef USE_AIRSPEED_LOWPASS_FILTER
  init_butterworth_2_low_pass(&ms45xx_filter, ms45xx.lowpass_tau,
                            MS45XX_I2C_PERIODIC_PERIOD, get_butterworth_2_low_pass(&ms45xx_filter));
#endif
}
 
 
void ms45xx_i2c_periodic(void)
{
  // Initiate next read
  if (ms45xx_trans.status == I2CTransDone) {
    i2c_receive(&MS45XX_I2C_DEV, &ms45xx_trans, MS45XX_I2C_ADDR, 4);
  }
}
 
#define AUTOSET_NB_MAX 200
 
void ms45xx_i2c_event(void)
{
  static int autoset_nb = 0;
  static float autoset_offset = 0.f;
 
  /* Check if transaction is succesfull */
  if (ms45xx_trans.status == I2CTransSuccess) {
 
    /* 2 MSB of data are status bits, 0 = good data, 2 = already fetched, 3 = fault */
    uint8_t status = (0xC0 & ms45xx_trans.buf[0]) >> 6;
 
    if (status == 0) {
      /* 14bit raw pressure */
      uint16_t p_raw = 0x3FFF & (((uint16_t)(ms45xx_trans.buf[0]) << 8) | (uint16_t)(ms45xx_trans.buf[1]));
      ms45xx.raw_p = p_raw;
 
      /* 11bit raw temperature, 5 LSB bits not used */
      uint16_t temp_raw = 0xFFE0 & (((uint16_t)(ms45xx_trans.buf[2]) << 8) |
                                    (uint16_t)(ms45xx_trans.buf[3]));
      temp_raw = temp_raw >> 5;
 
      /* Reject any values that are the absolute minimum or maximums these
         can happen due to gnd lifts or communication errors on the bus */
      if(p_raw == 0x3FFF || p_raw == 0 || temp_raw == 0x7FF || temp_raw == 0) {
        ms45xx_trans.status = I2CTransDone;
        return;
      }
 
      /* For type Diff
       * Output is proportional to the difference between Port 1 and Port 2. Output
       * swings positive when Port 1> Port 2. Output is 50% of total counts
       * when Port 1=Port 2.
       * For type Gauge
       * p_out = (p_raw - offset) * scale
       */
 
      float p_out = (p_raw - ms45xx.pressure_offset) * ms45xx.pressure_scale;
 
      /* 0 = -50degC, 20147 = 150degC
       * ms45xx_temperature in 0.1 deg Celcius
       */
      ms45xx.temperature = ((uint32_t)temp_raw * 2000) / 2047 - 500;
      
      // if(electrical.vboard != 0) {
      //   float volt_diff = electrical.vboard - 5.0f;
      //   Bound(volt_diff, -0.7f, 0.5f);
 
      //   p_out -= 65.0f * volt_diff;
      //   ms45xx.temperature -= 8.87f * volt_diff;
      // }
#ifdef USE_AIRSPEED_LOWPASS_FILTER
      ms45xx.pressure = update_butterworth_2_low_pass(&ms45xx_filter, p_out);
#else
      ms45xx.pressure = p_out;
#endif
 
      if (ms45xx.autoset_offset) {
        if (autoset_nb < AUTOSET_NB_MAX) {
          autoset_offset += p_raw;
          autoset_nb++;
        } else {
          ms45xx.pressure_offset = autoset_offset / (float)autoset_nb;
          autoset_offset = 0.f;
          autoset_nb = 0;
          ms45xx.autoset_offset = false;
          ms45xx.offset_set = true;
        }
      }
 
      // Send (differential) pressure via ABI
      #if AIRSPEED_MS45XX_SEND_ABI
      AbiSendMsgBARO_DIFF(MS45XX_SENDER_ID, ms45xx.pressure);
 
      // Send temperature as float in deg Celcius via ABI
      float temp = ms45xx.temperature / 10.0f;
      
      AbiSendMsgTEMPERATURE(MS45XX_SENDER_ID, temp);
      #endif
 
      // Compute airspeed
      float sign = 1.0f;
      if (ms45xx.pressure < 0.0f) {
        sign = -1.0f;
      }
      ms45xx.airspeed = sqrtf(ms45xx.pressure * sign * MS45XX_RHO_DIV_2) * sign;
 
    }
 
    // Set to done
    ms45xx_trans.status = I2CTransDone;
  } else if (ms45xx_trans.status == I2CTransFailed) {
    // Just retry if failed
    ms45xx_trans.status = I2CTransDone;
  }
}