text: /****************************************************************************
   *
   *   Copyright (c) 2013-2021 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 "ETSAirspeed.hpp"
  
  ETSAirspeed::ETSAirspeed(const I2CSPIDriverConfig &config) :
  	I2C(config),
  	I2CSPIDriver(config)
  {
  }
  
  ETSAirspeed::~ETSAirspeed()
  {
  	perf_free(_sample_perf);
  	perf_free(_comms_errors);
  }
  
  int ETSAirspeed::probe()
  {
  	_retries = 1;
  	int ret = measure();
  
  	return ret;
  }
  
  int ETSAirspeed::init()
  {
  	int ret = I2C::init();
  
  	if (ret != PX4_OK) {
  		DEVICE_DEBUG("I2C::init failed (%i)", ret);
  		return ret;
  	}
  
  	if (ret == PX4_OK) {
  		ScheduleNow();
  	}
  
  	return ret;
  }
  
  int ETSAirspeed::measure()
  {
  	/*
  	 * Send the command to begin a measurement.
  	 */
  	uint8_t cmd = READ_CMD;
  	int ret = transfer(&cmd, 1, nullptr, 0);
  
  	if (OK != ret) {
  		perf_count(_comms_errors);
  	}
  
  	return ret;
  }
  
  int ETSAirspeed::collect()
  {
  	/* read from the sensor */
  	uint8_t val[2] = {0, 0};
  
  	perf_begin(_sample_perf);
  	const hrt_abstime timestamp_sample = hrt_absolute_time();
  	int ret = transfer(nullptr, 0, &val[0], 2);
  
  	if (ret < 0) {
  		perf_count(_comms_errors);
  		return ret;
  	}
  
  	float diff_press_pa = (float)(val[1] << 8 | val[0]);
  
  	if (diff_press_pa < FLT_EPSILON) {
  		// a zero value indicates no measurement
  		// since the noise floor has been arbitrarily killed
  		// it defeats our stuck sensor detection - the best we
  		// can do is to output some numerical noise to show
  		// that we are still correctly sampling.
  		diff_press_pa = 0.001f * (timestamp_sample & 0x01);
  	}
  
  	differential_pressure_s differential_pressure{};
  	differential_pressure.timestamp_sample = timestamp_sample;
  	differential_pressure.device_id = get_device_id();
  	differential_pressure.differential_pressure_pa = diff_press_pa;
  	differential_pressure.temperature = NAN;
  	differential_pressure.error_count = perf_event_count(_comms_errors);
  	differential_pressure.timestamp = hrt_absolute_time();
  	_differential_pressure_pub.publish(differential_pressure);
  
  	perf_end(_sample_perf);
  
  	return PX4_OK;
  }
  
  void ETSAirspeed::RunImpl()
  {
  	int ret;
  
  	/* collection phase? */
  	if (_collect_phase) {
  
  		/* perform collection */
  		ret = collect();
  
  		if (OK != ret) {
  			perf_count(_comms_errors);
  			/* restart the measurement state machine */
  			_collect_phase = false;
  			_sensor_ok = false;
  			ScheduleNow();
  			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);
  }
  