自学教程：C++ DEVICE_DEBUG函数代码示例

intHMC5883_I2C::probe(){	uint8_t data[3] = {0, 0, 0};	_retries = 10;	if (read(ADDR_ID_A, &data[0], 1) ||	    read(ADDR_ID_B, &data[1], 1) ||	    read(ADDR_ID_C, &data[2], 1)) {		DEVICE_DEBUG("read_reg fail");		return -EIO;	}	_retries = 2;	if ((data[0] != ID_A_WHO_AM_I) ||	    (data[1] != ID_B_WHO_AM_I) ||	    (data[2] != ID_C_WHO_AM_I)) {		DEVICE_DEBUG("ID byte mismatch (%02x,%02x,%02x)", data[0], data[1], data[2]);		return -EIO;	}	return OK;}

voidLIS3MDL::cycle(){	/* _measure_ticks == 0  is used as _task_should_exit */	if (_measure_ticks == 0) {		return;	}	/* Collect last measurement at the start of every cycle */	if (collect() != OK) {		DEVICE_DEBUG("collection error");		/* restart the measurement state machine */		start();		return;	}	if (measure() != OK) {		DEVICE_DEBUG("measure error");	}	if (_measure_ticks > 0) {		/* schedule a fresh cycle call when the measurement is done */		work_queue(HPWORK,			   &_work,			   (worker_t)&LIS3MDL::cycle_trampoline,			   this,			   USEC2TICK(LIS3MDL_CONVERSION_INTERVAL));	}}

intLIS3MDL_SPI::init(){	int ret;	ret = SPI::init();	if (ret != OK) {		DEVICE_DEBUG("SPI init failed");		return -EIO;	}	// read WHO_AM_I value	uint8_t data = 0;	if (read(ADDR_WHO_AM_I, &data, 1)) {		DEVICE_DEBUG("LIS3MDL read_reg fail");	}	if (data != ID_WHO_AM_I) {		DEVICE_DEBUG("LIS3MDL bad ID: %02x", data);		return -EIO;	}	return OK;}

intLPS25H::init(){	int ret;	ret = CDev::init();	if (ret != OK) {		DEVICE_DEBUG("CDev init failed");		goto out;	}	/* allocate basic report buffers */	_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_baro_s));	if (_reports == nullptr) {		DEVICE_DEBUG("can't get memory for reports");		ret = -ENOMEM;		goto out;	}	if (reset() != OK) {		goto out;	}	/* register alternate interfaces if we have to */	_class_instance = register_class_devname(BARO_BASE_DEVICE_PATH);	ret = OK;out:	return ret;}

intHMC5883_SPI::init(){	int ret;	ret = SPI::init();	if (ret != OK) {		DEVICE_DEBUG("SPI init failed");		return -EIO;	}	// read WHO_AM_I value	uint8_t data[3] = {0, 0, 0};	if (read(ADDR_ID_A, &data[0], 1) ||	    read(ADDR_ID_B, &data[1], 1) ||	    read(ADDR_ID_C, &data[2], 1)) {		DEVICE_DEBUG("read_reg fail");	}	if ((data[0] != ID_A_WHO_AM_I) ||	    (data[1] != ID_B_WHO_AM_I) ||	    (data[2] != ID_C_WHO_AM_I)) {		DEVICE_DEBUG("ID byte mismatch (%02x,%02x,%02x)", data[0], data[1], data[2]);		return -EIO;	}	return OK;}

voidPCA9685::subscribe(){	/* subscribe/unsubscribe to required actuator control groups */	uint32_t sub_groups = _groups_required & ~_groups_subscribed;	uint32_t unsub_groups = _groups_subscribed & ~_groups_required;	_poll_fds_num = 0;	for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {		if (sub_groups & (1 << i)) {			DEVICE_DEBUG("subscribe to actuator_controls_%d", i);			_control_subs[i] = orb_subscribe(_control_topics[i]);		}		if (unsub_groups & (1 << i)) {			DEVICE_DEBUG("unsubscribe from actuator_controls_%d", i);			::close(_control_subs[i]);			_control_subs[i] = -1;		}		if (_control_subs[i] > 0) {			_poll_fds[_poll_fds_num].fd = _control_subs[i];			_poll_fds[_poll_fds_num].events = POLLIN;			_poll_fds_num++;		}	}}

intLPS25H_SPI::init(){	int ret;	ret = SPI::init();	if (ret != OK) {		DEVICE_DEBUG("SPI init failed");		return -EIO;	}	// read WHO_AM_I value	uint8_t id;	if (read(ADDR_ID, &id, 1)) {		DEVICE_DEBUG("read_reg fail");		return -EIO;	}	if (id != ID_WHO_AM_I) {		DEVICE_DEBUG("ID byte mismatch (%02x != %02x)", ID_WHO_AM_I, id);		return -EIO;	}	return OK;}

intBMI055_gyro::init(){	int ret;	/* do SPI init (and probe) first */	ret = SPI::init();	/* if probe/setup failed, bail now */	if (ret != OK) {		DEVICE_DEBUG("SPI setup failed");		return ret;	}	/* allocate basic report buffers */	_gyro_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report));	if (_gyro_reports == nullptr) {		goto out;	}	if (reset() != OK) {		goto out;	}	/* Initialize offsets and scales */	_gyro_scale.x_offset = 0;	_gyro_scale.x_scale  = 1.0f;	_gyro_scale.y_offset = 0;	_gyro_scale.y_scale  = 1.0f;	_gyro_scale.z_offset = 0;	_gyro_scale.z_scale  = 1.0f;	/* if probe/setup failed, bail now */	if (ret != OK) {		DEVICE_DEBUG("gyro init failed");		return ret;	}	_gyro_class_instance = register_class_devname(GYRO_BASE_DEVICE_PATH);	measure();	/* advertise sensor topic, measure manually to initialize valid report */	struct gyro_report grp;	_gyro_reports->get(&grp);	_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,					  &_gyro_orb_class_instance, (external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1);	if (_gyro_topic == nullptr) {		warnx("ADVERT FAIL");	}out:	return ret;}

void LidarLiteI2C::cycle(){	/* collection phase? */	if (_collect_phase) {		/* try a collection */		if (OK != collect()) {			DEVICE_DEBUG("collection error");			/* if we've been waiting more than 200ms then			   send a new acquire */			if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT * 2) {				_collect_phase = false;			}		} else {			/* next phase is measurement */			_collect_phase = false;			/*			 * Is there a collect->measure gap?			 */			if (getMeasureTicks() > USEC2TICK(LL40LS_CONVERSION_INTERVAL)) {				/* schedule a fresh cycle call when we are ready to measure again */				work_queue(HPWORK,					   &_work,					   (worker_t)&LidarLiteI2C::cycle_trampoline,					   this,					   getMeasureTicks() - USEC2TICK(LL40LS_CONVERSION_INTERVAL));				return;			}		}	}	if (_collect_phase == false) {		/* measurement phase */		if (OK != measure()) {			DEVICE_DEBUG("measure error");		} else {			/* next phase is collection. Don't switch to			   collection phase until we have a successful			   acquire request I2C transfer */			_collect_phase = true;		}	}	/* schedule a fresh cycle call when the measurement is done */	work_queue(HPWORK,		   &_work,		   (worker_t)&LidarLiteI2C::cycle_trampoline,		   this,		   USEC2TICK(LL40LS_CONVERSION_INTERVAL));}

voidLIS3MDL::cycle(){	if (_measure_ticks == 0) {		return;	}	/* collection phase? */	if (_collect_phase) {		/* perform collection */		if (OK != collect()) {			DEVICE_DEBUG("collection error");			/* restart the measurement state machine */			start();			return;		}		/* next phase is measurement */		_collect_phase = false;		/*		 * Is there a collect->measure gap?		 */		if (_measure_ticks > USEC2TICK(LIS3MDL_CONVERSION_INTERVAL)) {			/* schedule a fresh cycle call when we are ready to measure again */			work_queue(HPWORK,				   &_work,				   (worker_t)&LIS3MDL::cycle_trampoline,				   this,				   _measure_ticks - USEC2TICK(LIS3MDL_CONVERSION_INTERVAL));			return;		}	}	/* measurement phase */	if (OK != measure()) {		DEVICE_DEBUG("measure error");	}	/* next phase is collection */	_collect_phase = true;	if (_measure_ticks > 0) {		/* schedule a fresh cycle call when the measurement is done */		work_queue(HPWORK,			   &_work,			   (worker_t)&LIS3MDL::cycle_trampoline,			   this,			   USEC2TICK(LIS3MDL_CONVERSION_INTERVAL));	}}

intToneAlarm::ioctl(file *filp, int cmd, unsigned long arg){	int result = OK;	DEVICE_DEBUG("ioctl %i %u", cmd, arg);//	irqstate_t flags = enter_critical_section();	/* decide whether to increase the alarm level to cmd or leave it alone */	switch (cmd) {	case TONE_SET_ALARM:		DEVICE_DEBUG("TONE_SET_ALARM %u", arg);		if (arg < TONE_NUMBER_OF_TUNES) {			if (arg == TONE_STOP_TUNE) {				// stop the tune				_tune = nullptr;				_next = nullptr;				_repeat = false;				_default_tune_number = 0;			} else {				/* always interrupt alarms, unless they are repeating and already playing */				if (!(_repeat && _default_tune_number == arg)) {					/* play the selected tune */					_default_tune_number = arg;					start_tune(_default_tunes[arg]);				}			}		} else {			result = -EINVAL;		}		break;	default:		result = -ENOTTY;		break;	}//	leave_critical_section(flags);	/* give it to the superclass if we didn't like it */	if (result == -ENOTTY) {		result = CDev::ioctl(filp, cmd, arg);	}	return result;}

int LidarLiteI2C::init(){	int ret = PX4_ERROR;	/* do I2C init (and probe) first */	if (I2C::init() != OK) {		return ret;	}	/* allocate basic report buffers */	_reports = new ringbuffer::RingBuffer(2, sizeof(struct distance_sensor_s));	if (_reports == nullptr) {		return ret;	}	_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);	/* get a publish handle on the range finder topic */	struct distance_sensor_s ds_report = {};	measure();	_reports->get(&ds_report);	_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,				 &_orb_class_instance, ORB_PRIO_LOW);	if (_distance_sensor_topic == nullptr) {		DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?");	}	ret = OK;	/* sensor is ok, but we don't really know if it is within range */	_sensor_ok = true;	return ret;}

intPX4FMU::ioctl(file *filp, int cmd, unsigned long arg){	int ret;	/* try it as a GPIO ioctl first */	ret = gpio_ioctl(filp, cmd, arg);	if (ret != -ENOTTY) {		return ret;	}	/* if we are in valid PWM mode, try it as a PWM ioctl as well */	switch (_mode) {	case MODE_2PWM:	case MODE_4PWM:	case MODE_6PWM:#ifdef CONFIG_ARCH_BOARD_AEROCORE	case MODE_8PWM:#endif		ret = pwm_ioctl(filp, cmd, arg);		break;	default:		DEVICE_DEBUG("not in a PWM mode");		break;	}	/* if nobody wants it, let CDev have it */	if (ret == -ENOTTY) {		ret = CDev::ioctl(filp, cmd, arg);	}	return ret;}

int LidarLiteI2C::probe(){	// cope with both old and new I2C bus address	const uint8_t addresses[2] = {LL40LS_BASEADDR, LL40LS_BASEADDR_OLD};	// more retries for detection	_retries = 10;	for (uint8_t i = 0; i < sizeof(addresses); i++) {		/*		  check for hw and sw versions. It would be better if		  we had a proper WHOAMI register		 */		if (read_reg(LL40LS_HW_VERSION, _hw_version) == OK && _hw_version > 0 &&		    read_reg(LL40LS_SW_VERSION, _sw_version) == OK && _sw_version > 0) {			goto ok;		}		DEVICE_DEBUG("probe failed hw_version=0x%02x sw_version=0x%02x/n",			     (unsigned)_hw_version,			     (unsigned)_sw_version);	}	// not found on any address	return -EIO;ok:	_retries = 3;	return reset_sensor();}

intQMC5883::init(){	int ret = PX4_ERROR;	ret = CDev::init();	if (ret != OK) {		DEVICE_DEBUG("CDev init failed");		goto out;	}	/* allocate basic report buffers */	_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report));	if (_reports == nullptr) {		goto out;	}	/* reset the device configuration */	reset();	_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH);	ret = OK;	/* sensor is ok */	_sensor_ok = true;out:	return ret;}

int LidarLitePWM::init(){	/* do regular cdev init */	int ret = CDev::init();	if (ret != PX4_OK) {		return PX4_ERROR;	}	/* allocate basic report buffers */	_reports = new ringbuffer::RingBuffer(2, sizeof(struct distance_sensor_s));	if (_reports == nullptr) {		return PX4_ERROR;	}	_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);	/* get a publish handle on the distance_sensor topic */	struct distance_sensor_s ds_report = {};	measure();	_reports->get(&ds_report);	_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,				 &_orb_class_instance, ORB_PRIO_LOW);	if (_distance_sensor_topic == nullptr) {		DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?");	}	return PX4_OK;}

intSRF02_I2C::measure(){	int ret;	/*	 * Send the command to begin a measurement.	 */	uint8_t cmd[2];	cmd[0] = 0x00;	cmd[1] = SRF02_TAKE_RANGE_REG;	ret = transfer(cmd, 2, nullptr, 0);	if (OK != ret) {		perf_count(_comms_errors);		DEVICE_DEBUG("i2c::transfer returned %d", ret);		return ret;	}	ret = OK;	return ret;}

/** * Main loop function */voidPCA9685::i2cpwm(){	if (_mode == IOX_MODE_TEST_OUT) {		setPin(0, PCA9685_PWMCENTER);		_should_run = true;	} else if (_mode == IOX_MODE_OFF) {		_should_run = false;	} else {		if (!_mode_on_initialized) {			/* Subscribe to actuator control 2 (payload group for gimbal) */			_actuator_controls_sub = orb_subscribe(ORB_ID(actuator_controls_2));			/* set the uorb update interval lower than the driver pwm interval */			orb_set_interval(_actuator_controls_sub, 1000.0f / PCA9685_PWMFREQ - 5);			_mode_on_initialized = true;		}		/* Read the servo setpoints from the actuator control topics (gimbal) */		bool updated;		orb_check(_actuator_controls_sub, &updated);		if (updated) {			orb_copy(ORB_ID(actuator_controls_2), _actuator_controls_sub, &_actuator_controls);			for (int i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {				/* Scale the controls to PWM, first multiply by pi to get rad,				 * the control[i] values are on the range -1 ... 1 */				uint16_t new_value = PCA9685_PWMCENTER +						     (_actuator_controls.control[i] * M_PI_F * PCA9685_SCALE);				DEVICE_DEBUG("%d: current: %u, new %u, control %.2f", i, _current_values[i], new_value,					     (double)_actuator_controls.control[i]);				if (new_value != _current_values[i] &&				    isfinite(new_value) &&				    new_value >= PCA9685_PWMMIN &&				    new_value <= PCA9685_PWMMAX) {					/* This value was updated, send the command to adjust the PWM value */					setPin(i, new_value);					_current_values[i] = new_value;				}			}		}		_should_run = true;	}	// check if any activity remains, else stop	if (!_should_run) {		_running = false;		return;	}	// re-queue ourselves to run again later	_running = true;	work_queue(LPWORK, &_work, (worker_t)&PCA9685::i2cpwm_trampoline, this, _i2cpwm_interval);}

intADCSIM::init(){	DEVICE_DEBUG("init done");	/* create the device node */	return VDev::init();}

intSPI::init(){	int ret = OK;	/* attach to the spi bus */	if (_dev == nullptr) {		_dev = up_spiinitialize(_bus);	}	if (_dev == nullptr) {		DEVICE_DEBUG("failed to init SPI");		ret = -ENOENT;		goto out;	}	// tell other SPI users that we may be doing transfers in	// interrupt context	up_spi_set_need_irq_save(_dev);	/* deselect device to ensure high to low transition of pin select */	SPI_SELECT(_dev, _device, false);	/* call the probe function to check whether the device is present */	ret = probe();	if (ret != OK) {		DEVICE_DEBUG("probe failed");		goto out;	}	/* do base class init, which will create the device node, etc. */	ret = CDev::init();	if (ret != OK) {		DEVICE_DEBUG("cdev init failed");		goto out;	}	/* tell the workd where we are */	DEVICE_LOG("on SPI bus %d at %d (%u KHz)", _bus, _device, _frequency / 1000);out:	return ret;}

intTAP_ESC::ioctl(file *filp, int cmd, unsigned long arg){	int ret = OK;	switch (cmd) {	case MIXERIOCRESET:		if (_mixers != nullptr) {			delete _mixers;			_mixers = nullptr;			_groups_required = 0;		}		break;	case MIXERIOCLOADBUF: {			const char *buf = (const char *)arg;			unsigned buflen = strnlen(buf, 1024);			if (_mixers == nullptr) {				_mixers = new MixerGroup(control_callback, (uintptr_t)_controls);			}			if (_mixers == nullptr) {				_groups_required = 0;				ret = -ENOMEM;			} else {				ret = _mixers->load_from_buf(buf, buflen);				if (ret != 0) {					DEVICE_DEBUG("mixer load failed with %d", ret);					delete _mixers;					_mixers = nullptr;					_groups_required = 0;					ret = -EINVAL;				} else {					_mixers->groups_required(_groups_required);				}			}			break;		}	default:		ret = -ENOTTY;		break;	}	return ret;}

voidETSAirspeed::cycle(){	int ret;	/* collection phase? */	if (_collect_phase) {		/* perform collection */		ret = collect();		if (OK != ret) {			perf_count(_comms_errors);			/* restart the measurement state machine */			start();			_sensor_ok = false;			return;		}		/* next phase is measurement */		_collect_phase = false;		/*		 * Is there a collect->measure gap?		 */		if (_measure_ticks > USEC2TICK(CONVERSION_INTERVAL)) {			/* schedule a fresh cycle call when we are ready to measure again */			work_queue(HPWORK,				   &_work,				   (worker_t)&Airspeed::cycle_trampoline,				   this,				   _measure_ticks - USEC2TICK(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 */	work_queue(HPWORK,		   &_work,		   (worker_t)&Airspeed::cycle_trampoline,		   this,		   USEC2TICK(CONVERSION_INTERVAL));}

voidPX4FLOW::cycle(){	if (OK != measure()) {		DEVICE_DEBUG("measure error");	}	/* perform collection */	if (OK != collect()) {		DEVICE_DEBUG("collection error");		/* restart the measurement state machine */		start();		return;	}	work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this,		   _measure_ticks);}

intI2C::init(){	int ret = PX4_OK;	// Assume the driver set the desired bus frequency. There is no standard	// way to set it from user space.	// do base class init, which will create device node, etc	ret = VDev::init();	if (ret != PX4_OK) {		DEVICE_DEBUG("VDev::init failed");		return ret;	}	_fd = px4_open(get_devname(), PX4_F_RDONLY | PX4_F_WRONLY);	if (_fd < 0) {		DEVICE_DEBUG("px4_open failed of device %s", get_devname());		return PX4_ERROR;	}#ifdef __PX4_QURT	simulate = true;#endif	if (simulate) {		_fd = 10000;	}	else {#ifndef __PX4_QURT		// Open the actual I2C device and map to the virtual dev name		_fd = ::open(get_devname(), O_RDWR);		if (_fd < 0) {			warnx("could not open %s", get_devname());			px4_errno = errno;			return PX4_ERROR;		}#endif	}	return ret;}

intRM3100_I2C::probe(){	uint8_t data = 0;	_retries = 10;	if (read(ADDR_REVID, &data, 1)) {		DEVICE_DEBUG("RM3100 read_reg fail");		return -EIO;	}	_retries = 2;	if (data != RM3100_REVID) {		DEVICE_DEBUG("RM3100 bad ID: %02x", data);		return -EIO;	}	return OK;}

intLPS22HB_I2C::probe(){	uint8_t id;	_retries = 10;	if (read(WHO_AM_I, &id, 1)) {		DEVICE_DEBUG("read_reg fail");		return -EIO;	}	_retries = 2;	if (id != LPS22HB_ID_WHO_AM_I) {		DEVICE_DEBUG("ID byte mismatch (%02x != %02x)", LPS22HB_ID_WHO_AM_I, id);		return -EIO;	}	return OK;}

intIST8310::probe(){	uint8_t data[1] = {0};	_retries = 10;	if (read(ADDR_WAI, &data[0], 1)) {		DEVICE_DEBUG("read_reg fail");		return -EIO;	}	_retries = 2;	if ((data[0] != WAI_EXPECTED_VALUE)) {		DEVICE_DEBUG("ID byte mismatch (%02x) expected %02x", data[0], WAI_EXPECTED_VALUE);		return -EIO;	}	return OK;}

intSRF02_I2C::collect(){	int	ret = -EIO;	/* read from the sensor */	uint8_t val[2] = {0, 0};	uint8_t cmd = 0x02;	perf_begin(_sample_perf);	ret = transfer(&cmd, 1, nullptr, 0);	ret = transfer(nullptr, 0, &val[0], 2);	if (ret < 0) {		DEVICE_DEBUG("error reading from sensor: %d", ret);		perf_count(_comms_errors);		perf_end(_sample_perf);		return ret;	}	uint16_t distance_cm = val[0] << 8 | val[1];	float distance_m = float(distance_cm) * 1e-2f;	struct distance_sensor_s report;	report.timestamp = hrt_absolute_time();	report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;	report.orientation = 8;	report.current_distance = distance_m;	report.min_distance = get_minimum_distance();	report.max_distance = get_maximum_distance();	report.covariance = 0.0f;	/* TODO: set proper ID */	report.id = 0;	/* publish it, if we are the primary */	if (_distance_sensor_topic != nullptr) {		orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);	}	if (_reports->force(&report)) {		perf_count(_buffer_overflows);	}	/* notify anyone waiting for data */	poll_notify(POLLIN);	ret = OK;	perf_end(_sample_perf);	return ret;}

intMPU6000_SPI::init(){	int ret;	ret = SPI::init();	if (ret != OK) {		DEVICE_DEBUG("SPI init failed");		return -EIO;	}	return OK;}