uint8_t isl28022_init()

in common/sensor/dev/isl28022.c [74:130]

• 48 lines of code
• 7 McCabe index (conditional complexity)

uint8_t isl28022_init(uint8_t sensor_num)
{
	if (sensor_config[SensorNum_SensorCfg_map[sensor_num]].init_args == NULL) {
		printk("isl28022_init: init_arg is NULL\n");
		return SENSOR_INIT_UNSPECIFIED_ERROR;
	}

	sensor_config[SensorNum_SensorCfg_map[sensor_num]].read = isl28022_read;
	isl28022_init_arg *init_arg =
		(isl28022_init_arg *)sensor_config[SensorNum_SensorCfg_map[sensor_num]].init_args;
	if (init_arg->is_init == true) {
		return SENSOR_INIT_SUCCESS;
	}

	I2C_MSG msg;
	uint8_t retry = 5;

	/* set configuration register */
	msg.bus = sensor_config[SensorNum_SensorCfg_map[sensor_num]].port;
	msg.slave_addr = sensor_config[SensorNum_SensorCfg_map[sensor_num]].slave_addr;
	msg.tx_len = 3;
	msg.data[0] = ISL28022_CONFIG_REG;
	msg.data[1] = (init_arg->config.value >> 8) & 0xFF;
	msg.data[2] = init_arg->config.value & 0xFF;
	if (i2c_master_write(&msg, retry)) {
		printk("isl28022_init, set configuration register fail\n");
		return SENSOR_INIT_UNSPECIFIED_ERROR;
	}

	/* calculate and set calibration */
	uint16_t v_shunt_fs, adc_res, calibration;

	v_shunt_fs = 40 << (init_arg->config.fields.PG);
	if (!(init_arg->config.fields.SADC & BIT(3)) &&
	    ((init_arg->config.fields.SADC & BIT_MASK(2)) < 3)) {
		adc_res = 1 << (12 + (init_arg->config.fields.SADC & BIT_MASK(2)));
	} else {
		adc_res = 32768;
	}
	init_arg->current_LSB = (float)v_shunt_fs / (init_arg->r_shunt * adc_res);
	calibration = (40.96 / (init_arg->current_LSB * init_arg->r_shunt));
	calibration = calibration << 1; /* 15 bits, bit[0] is fix to 0 */

	msg.bus = sensor_config[SensorNum_SensorCfg_map[sensor_num]].port;
	msg.slave_addr = sensor_config[SensorNum_SensorCfg_map[sensor_num]].slave_addr;
	msg.tx_len = 3;
	msg.data[0] = ISL28022_CALIBRATION_REG;
	msg.data[1] = (calibration >> 8) & 0xFF;
	msg.data[2] = calibration & 0xFF;
	if (i2c_master_write(&msg, retry)) {
		printk("isl28022_init, set calibration register fail\n");
		return SENSOR_INIT_UNSPECIFIED_ERROR;
	}

	init_arg->is_init = true;
	return SENSOR_INIT_SUCCESS;
}