text: /****************************************************************************
   *
   *   Copyright (c) 2019 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.
   *
   ****************************************************************************/
  
  /**
   * @file ina226.cpp
   * @author David Sidrane <david_s5@usa.net>
   *
   * Driver for the I2C attached INA226
   */
  #define INA226_RAW // remove this
  
  #include "ina226.h"
  
  #include <string.h>
  
  #include <px4_platform_common/px4_config.h>
  #include <px4_platform_common/getopt.h>
  #include <px4_platform_common/module.h>
  #include <drivers/device/i2c.h>
  #include <lib/parameters/param.h>
  #include <lib/perf/perf_counter.h>
  #include <drivers/drv_hrt.h>
  #include <uORB/PublicationMulti.hpp>
  #include <uORB/topics/power_monitor.h>
  #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
  
  
  INA226::INA226(int battery_index, int bus, int address) :
  	I2C("INA226", nullptr, i2c_bus_options[bus], address, 100000),
  	ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
  	ModuleParams(nullptr),
  	index(bus),
  	_sample_perf(perf_alloc(PC_ELAPSED, "ina226_read")),
  	_comms_errors(perf_alloc(PC_COUNT, "ina226_com_err")),
  	_collection_errors(perf_alloc(PC_COUNT, "ina226_collection_err")),
  	_measure_errors(perf_alloc(PC_COUNT, "ina226_measurement_err")),
  	_battery(battery_index, this)
  {
  	float fvalue = MAX_CURRENT;
  	_max_current = fvalue;
  	param_t ph = param_find("INA226_CURRENT");
  
  	if (ph != PARAM_INVALID && param_get(ph, &fvalue) == PX4_OK) {
  		_max_current = fvalue;
  	}
  
  	fvalue = INA226_SHUNT;
  	_rshunt = fvalue;
  	ph = param_find("INA226_SHUNT");
  
  	if (ph != PARAM_INVALID && param_get(ph, &fvalue) == PX4_OK) {
  		_rshunt = fvalue;
  	}
  
  	ph = param_find("INA226_CONFIG");
  	int32_t value = INA226_CONFIG;
  	_config = (uint16_t)value;
  
  	if (ph != PARAM_INVALID && param_get(ph, &value) == PX4_OK) {
  		_config = (uint16_t)value;
  	}
  
  	_mode_triggered = ((_config & INA226_MODE_MASK) >> INA226_MODE_SHIFTS) <=
  			  ((INA226_MODE_SHUNT_BUS_TRIG & INA226_MODE_MASK) >>
  			   INA226_MODE_SHIFTS);
  
  	_current_lsb = _max_current / DN_MAX;
  	_power_lsb = 25 * _current_lsb;
  
  	// We need to publish immediately, to guarantee that the first instance of the driver publishes to uORB instance 0
  	_battery.updateBatteryStatus(
  		hrt_absolute_time(),
  		0.0,
  		0.0,
  		false,
  		false, // TODO: selected source?
  		0,
  		0.0,
  		true
  	);
  }
  
  INA226::~INA226()
  {
  	/* make sure we are truly inactive */
  	stop();
  
  	/* free perf counters */
  	perf_free(_sample_perf);
  	perf_free(_comms_errors);
  	perf_free(_collection_errors);
  	perf_free(_measure_errors);
  }
  
  int INA226::read(uint8_t address)
  {
  	union {
  		uint16_t reg;
  		uint8_t  b[2] = {};
  	} data;
  
  	int ret = transfer(&address, 1, &data.b[0], sizeof(data.b));
  
  	if (OK != ret) {
  		perf_count(_comms_errors);
  		PX4_DEBUG("i2c::transfer returned %d", ret);
  		return -1;
  	}
  
  	return swap16(data.reg);
  }
  
  int INA226::write(uint8_t address, uint16_t value)
  {
  	uint8_t data[3] = {address, ((uint8_t)((value & 0xff00) >> 8)), (uint8_t)(value & 0xff)};
  	return transfer(data, sizeof(data), nullptr, 0);
  }
  
  int
  INA226::init()
  {
  	int ret = PX4_ERROR;
  
  	/* do I2C init (and probe) first */
  	if (I2C::init() != OK) {
  		return ret;
  	}
  
  	write(INA226_REG_CONFIGURATION, INA226_RST);
  
  	_cal = INA226_CONST / (_current_lsb * INA226_SHUNT);
  
  	if (write(INA226_REG_CALIBRATION, _cal) < 0) {
  		return -3;
  	}
  
  	// If we run in continuous mode then start it here
  
  	if (!_mode_triggered) {
  		ret = write(INA226_REG_CONFIGURATION, _config);
  
  	} else {
  		ret = OK;
  	}
  
  	set_device_address(INA226_BASEADDR);	/* set I2c Address */
  
  	start();
  	_sensor_ok = true;
  
  	_initialized = ret == OK;
  	return ret;
  }
  
  int
  INA226::force_init()
  {
  	int ret = init();
  
  	start();
  
  	return ret;
  }
  
  int
  INA226::probe()
  {
  	int value = read(INA226_MFG_ID);
  
  	if (value < 0) {
  		perf_count(_comms_errors);
  	}
  
  	if (value != INA226_MFG_ID_TI) {
  		PX4_DEBUG("probe mfgid %d", value);
  		return -1;
  	}
  
  	value = read(INA226_MFG_DIEID);
  
  	if (value < 0) {
  		perf_count(_comms_errors);
  	}
  
  	if (value != INA226_MFG_DIE) {
  		PX4_DEBUG("probe die id %d", value);
  		return -1;
  	}
  
  	return OK;
  }
  
  int
  INA226::measure()
  {
  	int ret = OK;
  
  	if (_mode_triggered) {
  		ret = write(INA226_REG_CONFIGURATION, _config);
  
  		if (ret < 0) {
  			perf_count(_comms_errors);
  			PX4_DEBUG("i2c::transfer returned %d", ret);
  		}
  	}
  
  	return ret;
  }
  
  int
  INA226::collect()
  {
  	int ret = -EIO;
  
  	/* read from the sensor */
  	perf_begin(_sample_perf);
  
  	if (_initialized) {
  
  		_bus_voltage = read(INA226_REG_BUSVOLTAGE);
  		_power = read(INA226_REG_POWER);
  		_current = read(INA226_REG_CURRENT);
  		_shunt = read(INA226_REG_SHUNTVOLTAGE);
  
  	} else {
  		init();
  
  		_bus_voltage = -1.0f;
  		_power = -1.0f;
  		_current = -1.0f;
  		_shunt = -1.0f;
  	}
  
  	parameter_update_s param_update;
  
  	if (_parameters_sub.copy(&param_update)) {
  		// Currently, this INA226 driver doesn't really use ModuleParams. This call to updateParams() is just to
  		// update the battery, which is registered as a child.
  		updateParams();
  	}
  
  	// Note: If the power module is connected backwards, then the values of _power, _current, and _shunt will
  	//  be negative but otherwise valid. This isn't important, because why should we support the case where
  	//  the power module is used incorrectly?
  	if (_bus_voltage >= 0 && _power >= 0 && _current >= 0 && _shunt >= 0) {
  
  		_actuators_sub.copy(&_actuator_controls);
  
  		/* publish it */
  		_battery.updateBatteryStatus(
  			hrt_absolute_time(),
  			(float) _bus_voltage * INA226_VSCALE,
  			(float) _current * _current_lsb,
  			true,
  			true, // TODO: Determine if this is the selected source
  			0,
  			_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE],
  			true
  		);
  
  		ret = OK;
  
  	} else {
  		_battery.updateBatteryStatus(
  			hrt_absolute_time(),
  			0.0,
  			0.0,
  			false,
  			false, // TODO: selected source?
  			0,
  			0.0,
  			true
  		);
  		ret = -1;
  		perf_count(_comms_errors);
  	}
  
  	if (ret != OK) {
  		PX4_DEBUG("error reading from sensor: %d", ret);
  	}
  
  	perf_count(_comms_errors);
  	perf_end(_sample_perf);
  	return ret;
  }
  
  void
  INA226::start()
  {
  	ScheduleClear();
  
  	/* reset the report ring and state machine */
  	_collect_phase = false;
  
  	_measure_interval = INA226_CONVERSION_INTERVAL;
  
  	/* schedule a cycle to start things */
  	ScheduleDelayed(5);
  }
  
  void
  INA226::stop()
  {
  	ScheduleClear();
  }
  
  void
  INA226::Run()
  {
  	if (_initialized) {
  		if (_collect_phase) {
  
  			/* perform collection */
  			if (OK != collect()) {
  				perf_count(_collection_errors);
  				/* if error restart the measurement state machine */
  				start();
  				return;
  			}
  
  			/* next phase is measurement */
  			_collect_phase = !_mode_triggered;
  
  			if (_measure_interval > INA226_CONVERSION_INTERVAL) {
  
  				/* schedule a fresh cycle call when we are ready to measure again */
  				ScheduleDelayed(_measure_interval - INA226_CONVERSION_INTERVAL);
  				return;
  			}
  		}
  
  		/* Measurement  phase */
  
  		/* Perform measurement */
  		if (OK != measure()) {
  			perf_count(_measure_errors);
  		}
  
  		/* next phase is collection */
  		_collect_phase = true;
  
  		/* schedule a fresh cycle call when the measurement is done */
  		ScheduleDelayed(INA226_CONVERSION_INTERVAL);
  
  	} else {
  		_battery.updateBatteryStatus(
  			hrt_absolute_time(),
  			0.0f,
  			0.0f,
  			false,
  			false,
  			0,
  			0.0f,
  			true
  		);
  
  		if (init() != OK) {
  			ScheduleDelayed(INA226_INIT_RETRY_INTERVAL_US);
  		}
  	}
  }
  
  void
  INA226::print_info()
  {
  	if (_initialized) {
  		perf_print_counter(_sample_perf);
  		perf_print_counter(_comms_errors);
  
  		printf("poll interval:  %u \n", _measure_interval);
  
  	} else {
  		PX4_INFO("Device not initialized. Retrying every %d ms until battery is plugged in.",
  			 INA226_INIT_RETRY_INTERVAL_US);
  	}
  }
  