INA226: revise read() and collect() error handling

to allow negative currents and simplify unnecessary redundancy.

src/drivers/power_monitor/ina226/ina226.cpp

@@ -103,22 +103,21 @@ INA226::~INA226()
 	perf_free(_measure_errors);
 }
 
-int INA226::read(uint8_t address)
+int INA226::read(uint8_t address, int16_t &data)
 {
-	union {
+	// read desired little-endian value via I2C
-		uint16_t reg;
+	uint16_t received_bytes;
-		uint8_t  b[2] = {};
+	const int ret = transfer(&address, 1, (uint8_t *)&received_bytes, sizeof(received_bytes));
-	} data;
 
-	int ret = transfer(&address, 1, &data.b[0], sizeof(data.b));
+	if (ret == PX4_OK) {
+		data = swap16(received_bytes);
 
-	if (ret != PX4_OK) {
+	} else {
 		perf_count(_comms_errors);
 		PX4_DEBUG("i2c::transfer returned %d", ret);
-		return -1;
 	}
 
-	return swap16(data.reg);
+	return ret;
 }
 
 int INA226::write(uint8_t address, uint16_t value)
@@ -174,24 +173,14 @@ INA226::force_init()
 int
 INA226::probe()
 {
-	int value = read(INA226_MFG_ID);
+	int16_t value{0};
-
-	if (value < 0) {
-		perf_count(_comms_errors);
-	}
 
-	if (value != INA226_MFG_ID_TI) {
+	if (read(INA226_MFG_ID, value) != PX4_OK || value != INA226_MFG_ID_TI) {
 		PX4_DEBUG("probe mfgid %d", value);
 		return -1;
 	}
 
-	value = read(INA226_MFG_DIEID);
+	if (read(INA226_MFG_DIEID, value) != PX4_OK || value != INA226_MFG_DIE) {
-
-	if (value < 0) {
-		perf_count(_comms_errors);
-	}
-
-	if (value != INA226_MFG_DIE) {
 		PX4_DEBUG("probe die id %d", value);
 		return -1;
 	}
@@ -219,75 +208,47 @@ INA226::measure()
 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
+	// read from the sensor
-	//  be negative but otherwise valid. This isn't important, because why should we support the case where
+	// Note: If the power module is connected backwards, then the values of _power, _current, and _shunt will be negative but otherwise valid.
-	//  the power module is used incorrectly?
+	bool success{true};
-	if (_bus_voltage >= 0 && _power >= 0 && _current >= 0 && _shunt >= 0) {
+	success = success && (read(INA226_REG_BUSVOLTAGE, _bus_voltage) == PX4_OK);
+	// success = success && (read(INA226_REG_POWER, _power) == PX4_OK);
+	success = success && (read(INA226_REG_CURRENT, _current) == PX4_OK);
+	// success = success && (read(INA226_REG_SHUNTVOLTAGE, _shunt) == PX4_OK);
+
+	if (!success) {
+		PX4_DEBUG("error reading from sensor");
+		_bus_voltage = _power = _current = _shunt = 0;
+	}
 
-		_actuators_sub.copy(&_actuator_controls);
+	_actuators_sub.copy(&_actuator_controls);
 
-		/* publish it */
+	_battery.updateBatteryStatus(
-		_battery.updateBatteryStatus(
+		hrt_absolute_time(),
-			hrt_absolute_time(),
+		(float) _bus_voltage * INA226_VSCALE,
-			(float) _bus_voltage * INA226_VSCALE,
+		(float) _current * _current_lsb,
-			(float) _current * _current_lsb,
+		success,
-			true,
+		battery_status_s::BATTERY_SOURCE_POWER_MODULE,
-			battery_status_s::BATTERY_SOURCE_POWER_MODULE,
+		0,
-			0,
+		_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE]
-			_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE]
+	);
-		);
 
-		ret = PX4_OK;
+	perf_end(_sample_perf);
 
-	} else {
+	if (success) {
-		_battery.updateBatteryStatus(
+		return PX4_OK;
-			hrt_absolute_time(),
-			0.0,
-			0.0,
-			false,
-			battery_status_s::BATTERY_SOURCE_POWER_MODULE,
-			0,
-			0.0
-		);
-		ret = -1;
-	}
 
-	if (ret != PX4_OK) {
+	} else {
-		PX4_DEBUG("error reading from sensor: %d", ret);
+		return PX4_ERROR;
-		perf_count(_comms_errors);
 	}
-
-	perf_end(_sample_perf);
-	return ret;
 }
 
 void

src/drivers/power_monitor/ina226/ina226.h

@@ -153,8 +153,8 @@ private:
 	perf_counter_t 		_measure_errors;
 
 	int16_t           _bus_voltage{0};
-	int16_t           _power{-1};
+	int16_t           _power{0};
-	int16_t           _current{-1};
+	int16_t           _current{0};
 	int16_t           _shunt{0};
 	int16_t           _cal{0};
 	bool              _mode_triggered{false};
@@ -171,7 +171,7 @@ private:
 	uORB::Subscription  _actuators_sub{ORB_ID(actuator_controls_0)};
 	uORB::Subscription  _parameters_sub{ORB_ID(parameter_update)};
 
-	int read(uint8_t address);
+	int read(uint8_t address, int16_t &data);
 	int write(uint8_t address, uint16_t data);
 
 	/**