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

int UavcanBarometerBridge::init(){	int res = device::CDev::init();	if (res < 0) {		return res;	}	/* allocate basic report buffers */	_reports = new ringbuffer::RingBuffer(2, sizeof(baro_report));	if (_reports == nullptr) {		return -1;	}	res = _sub_air_pressure_data.start(AirPressureCbBinder(this, &UavcanBarometerBridge::air_pressure_sub_cb));	if (res < 0) {		DEVICE_LOG("failed to start uavcan sub: %d", res);		return res;	}	res = _sub_air_temperature_data.start(AirTemperatureCbBinder(this, &UavcanBarometerBridge::air_temperature_sub_cb));	if (res < 0) {		DEVICE_LOG("failed to start uavcan sub: %d", res);		return res;	}	return 0;}

intPX4FLOW::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(optical_flow_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 = {};	if (_class_instance == CLASS_DEVICE_PRIMARY) {		_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,					 &_orb_class_instance, ORB_PRIO_HIGH);		if (_distance_sensor_topic == nullptr) {			DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");		}	} else {		DEVICE_LOG("not primary range device, not advertising");	}	ret = OK;	/* sensor is ok, but we don't really know if it is within range */	_sensor_ok = true;	/* get yaw rotation from sensor frame to body frame */	param_t rot = param_find("SENS_FLOW_ROT");	/* only set it if the parameter exists */	if (rot != PARAM_INVALID) {		int32_t val = 6; // the recommended installation for the flow sensor is with the Y sensor axis forward		param_get(rot, &val);		_sensor_rotation = (enum Rotation)val;	}	return ret;}

intPMW3901::readMotionCount(int16_t &deltaX, int16_t &deltaY){	int ret;	uint8_t data[10] = { DIR_READ(0x02), 0, DIR_READ(0x03), 0, DIR_READ(0x04), 0,			     DIR_READ(0x05), 0, DIR_READ(0x06), 0			   };	ret = transfer(&data[0], &data[0], 10);	if (OK != ret) {		perf_count(_comms_errors);		DEVICE_LOG("spi::transfer returned %d", ret);		return ret;	}	deltaX = ((int16_t)data[5] << 8) | data[3];	deltaY = ((int16_t)data[9] << 8) | data[7];	ret = OK;	return ret;}

/* Wrapper to read a byte from addr */intPCA9685::read8(uint8_t addr, uint8_t &value){	int ret = OK;	/* send addr */	ret = transfer(&addr, sizeof(addr), nullptr, 0);	if (ret != OK) {		goto fail_read;	}	/* get value */	ret = transfer(nullptr, 0, &value, 1);	if (ret != OK) {		goto fail_read;	}	return ret;fail_read:	perf_count(_comms_errors);	DEVICE_LOG("i2c::transfer returned %d", ret);	return ret;}

uint16_tADC::_sample(unsigned channel){	perf_begin(_sample_perf);	/* clear any previous EOC */	if (rSR & ADC_SR_EOC) {		rSR &= ~ADC_SR_EOC;	}	/* run a single conversion right now - should take about 60 cycles (a few microseconds) max */	rSQR3 = channel;	rCR2 |= ADC_CR2_SWSTART;	/* wait for the conversion to complete */	hrt_abstime now = hrt_absolute_time();	while (!(rSR & ADC_SR_EOC)) {		/* don't wait for more than 50us, since that means something broke - should reset here if we see this */		if ((hrt_absolute_time() - now) > 50) {			DEVICE_LOG("sample timeout");			return 0xffff;		}	}	/* read the result and clear EOC */	uint16_t result = rDR;	perf_end(_sample_perf);	return result;}

voidTRONE::cycle(){	/* collection phase? */	if (_collect_phase) {		/* perform collection */		if (OK != collect()) {			DEVICE_LOG("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(TRONE_CONVERSION_INTERVAL)) {			/* schedule a fresh cycle call when we are ready to measure again */			work_queue(HPWORK,				   &_work,				   (worker_t)&TRONE::cycle_trampoline,				   this,				   _measure_ticks - USEC2TICK(TRONE_CONVERSION_INTERVAL));			return;		}	}	/* measurement phase */	if (OK != measure()) {		DEVICE_LOG("measure error");	}	/* next phase is collection */	_collect_phase = true;	/* schedule a fresh cycle call when the measurement is done */	work_queue(HPWORK,		   &_work,		   (worker_t)&TRONE::cycle_trampoline,		   this,		   USEC2TICK(TRONE_CONVERSION_INTERVAL));}

intTRONE::collect(){	int ret = -EIO;	/* read from the sensor */	uint8_t val[3] = {0, 0, 0};	perf_begin(_sample_perf);	ret = transfer(nullptr, 0, &val[0], 3);	if (ret < 0) {		DEVICE_LOG("error reading from sensor: %d", ret);		perf_count(_comms_errors);		perf_end(_sample_perf);		return ret;	}	uint16_t distance_mm = (val[0] << 8) | val[1];	float distance_m = float(distance_mm) *  1e-3f;	struct distance_sensor_s report;	report.timestamp = hrt_absolute_time();	/* there is no enum item for a combined LASER and ULTRASOUND which it should be */	report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;	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;	// This validation check can be used later	_valid = crc8(val, 2) == val[2] && (float)report.current_distance > report.min_distance		 && (float)report.current_distance < report.max_distance ? 1 : 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;}

int UavcanBarometerBridge::init(){	int res = device::CDev::init();	if (res < 0) {		return res;	}	res = _sub_air_pressure_data.start(AirPressureCbBinder(this, &UavcanBarometerBridge::air_pressure_sub_cb));	if (res < 0) {		DEVICE_LOG("failed to start uavcan sub: %d", res);		return res;	}	res = _sub_air_temperature_data.start(AirTemperatureCbBinder(this, &UavcanBarometerBridge::air_temperature_sub_cb));	if (res < 0) {		DEVICE_LOG("failed to start uavcan sub: %d", res);		return res;	}	return 0;}

void TAP_ESC::work_stop(){	for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {		if (_control_subs[i] > 0) {			orb_unsubscribe(_control_subs[i]);			_control_subs[i] = -1;		}	}	orb_unsubscribe(_armed_sub);	_armed_sub = -1;	orb_unsubscribe(_test_motor_sub);	_test_motor_sub = -1;	DEVICE_LOG("stopping");	_initialized = false;}

int UavcanMagnetometerBridge::init(){	int res = device::CDev::init();	if (res < 0) {		return res;	}	res = _sub_mag.start(MagCbBinder(this, &UavcanMagnetometerBridge::mag_sub_cb));	if (res < 0) {		DEVICE_LOG("failed to start uavcan sub: %d", res);		return res;	}	return 0;}

int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg){	switch (cmd) {	case BAROIOCSMSLPRESSURE: {			if ((arg < 80000) || (arg > 120000)) {				return -EINVAL;			} else {				DEVICE_LOG("new msl pressure %u", _msl_pressure);				_msl_pressure = arg;				return OK;			}		}	case BAROIOCGMSLPRESSURE: {			return _msl_pressure;		}	case SENSORIOCSPOLLRATE: {			// not supported yet, pretend that everything is ok			return OK;		}	case SENSORIOCSQUEUEDEPTH: {			/* lower bound is mandatory, upper bound is a sanity check */			if ((arg < 1) || (arg > 100)) {				return -EINVAL;			}			irqstate_t flags = irqsave();			if (!_reports->resize(arg)) {				irqrestore(flags);				return -ENOMEM;			}			irqrestore(flags);			return OK;		}	default: {			return CDev::ioctl(filp, cmd, arg);		}	}}

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;}

intSF0X::init(){	/* status */	int ret = 0;	do { /* create a scope to handle exit conditions using break */		/* do regular cdev init */		ret = CDev::init();		if (ret != OK) { break; }		/* allocate basic report buffers */		_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));		if (_reports == nullptr) {			warnx("mem err");			ret = -1;			break;		}		_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);		if (_class_instance == CLASS_DEVICE_PRIMARY) {			/* get a publish handle on the range finder topic */			struct distance_sensor_s ds_report = {};			_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,						 &_orb_class_instance, ORB_PRIO_HIGH);			if (_distance_sensor_topic == nullptr) {				DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");			}		}	} while (0);	/* close the fd */	::close(_fd);	_fd = -1;	return OK;}

intPMW3901::writeRegister(unsigned reg, uint8_t data){	uint8_t cmd[2]; 						// write 1 byte	int ret;	cmd[0] = DIR_WRITE(reg);	cmd[1] = data;	ret = transfer(&cmd[0], nullptr, 2);	if (OK != ret) {		perf_count(_comms_errors);		DEVICE_LOG("spi::transfer returned %d", ret);		return ret;	}	return ret;}

intSF0X::measure(){	int ret;	/*	 * Send the command to begin a measurement.	 */	char cmd = SF0X_TAKE_RANGE_REG;	ret = ::write(_fd, &cmd, 1);	if (ret != sizeof(cmd)) {		perf_count(_comms_errors);		DEVICE_LOG("write fail %d", ret);		return ret;	}	ret = OK;	return ret;}

intTRONE::measure(){	int ret;	/*	 * Send the command to begin a measurement.	 */	const uint8_t cmd = TRONE_MEASURE_REG;	ret = transfer(&cmd, sizeof(cmd), nullptr, 0);	if (OK != ret) {		perf_count(_comms_errors);		DEVICE_LOG("i2c::transfer returned %d", ret);		return ret;	}	ret = OK;	return ret;}

intTRONE::init(){	int ret = ERROR;	/* do I2C init (and probe) first */	if (I2C::init() != OK) {		goto out;	}	/* allocate basic report buffers */	_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));	if (_reports == nullptr) {		goto out;	}	_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);	if (_class_instance == CLASS_DEVICE_PRIMARY) {		/* 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_LOG("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;out:	return ret;}

void PX4FMU::work_stop(){	xTimerStop(_work, portMAX_DELAY);	for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {		if (_control_subs[i] > 0) {			::close(_control_subs[i]);			_control_subs[i] = -1;		}	}	::close(_armed_sub);	//::close(_param_sub);	/* make sure servos are off */	up_pwm_servo_deinit();	DEVICE_LOG("stopping/n");	/* note - someone else is responsible for restoring the GPIO config */	/* tell the dtor that we are exiting */	_initialized = false;}

intADC::init(){	/* do calibration if supported */#ifdef ADC_CR2_CAL	rCR2 |= ADC_CR2_CAL;	usleep(100);	if (rCR2 & ADC_CR2_CAL) {		return -1;	}#endif	/* arbitrarily configure all channels for 55 cycle sample time */	rSMPR1 = 0b00000011011011011011011011011011;	rSMPR2 = 0b00011011011011011011011011011011;	/* XXX for F2/4, might want to select 12-bit mode? */	rCR1 = 0;	/* enable the temperature sensor / Vrefint channel if supported*/	rCR2 =#ifdef ADC_CR2_TSVREFE		/* enable the temperature sensor in CR2 */		ADC_CR2_TSVREFE |#endif		0;#ifdef ADC_CCR_TSVREFE	/* enable temperature sensor in CCR */	rCCR = ADC_CCR_TSVREFE;#endif	/* configure for a single-channel sequence */	rSQR1 = 0;	rSQR2 = 0;	rSQR3 = 0;	/* will be updated with the channel each tick */	/* power-cycle the ADC and turn it on */	rCR2 &= ~ADC_CR2_ADON;	usleep(10);	rCR2 |= ADC_CR2_ADON;	usleep(10);	rCR2 |= ADC_CR2_ADON;	usleep(10);	/* kick off a sample and wait for it to complete */	hrt_abstime now = hrt_absolute_time();	rCR2 |= ADC_CR2_SWSTART;	while (!(rSR & ADC_SR_EOC)) {		/* don't wait for more than 500us, since that means something broke - should reset here if we see this */		if ((hrt_absolute_time() - now) > 500) {			DEVICE_LOG("sample timeout");			return -1;		}	}	DEVICE_DEBUG("init done");	/* create the device node */	return CDev::init();}

intSF1XX::init(){	int ret = PX4_ERROR;	int hw_model;	param_get(param_find("SENS_EN_SF1XX"), &hw_model);	switch (hw_model) {	case 0:		DEVICE_LOG("disabled.");		return ret;	case 1:  /* SF10/a (25m 32Hz) */		_min_distance = 0.01f;		_max_distance = 25.0f;		_conversion_interval = 31250;		break;	case 2:  /* SF10/b (50m 32Hz) */		_min_distance = 0.01f;		_max_distance = 50.0f;		_conversion_interval = 31250;		break;	case 3:  /* SF10/c (100m 16Hz) */		_min_distance = 0.01f;		_max_distance = 100.0f;		_conversion_interval = 62500;		break;	case 4:		/* SF11/c (120m 20Hz) */		_min_distance = 0.01f;		_max_distance = 120.0f;		_conversion_interval = 50000;		break;	case 5:		/* SF20/LW20 (100m 48-388Hz) */		_min_distance = 0.001f;		_max_distance = 100.0f;		_conversion_interval = 20834;		break;	default:		DEVICE_LOG("invalid HW model %d.", hw_model);		return ret;	}	/* do I2C init (and probe) first */	if (I2C::init() != OK) {		return ret;	}	/* allocate basic report buffers */	_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));	set_address(SF1XX_BASEADDR);	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 = {};	_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,				 &_orb_class_instance, ORB_PRIO_HIGH);	if (_distance_sensor_topic == nullptr) {		DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");	}	// Select altitude register	int ret2 = measure();	if (ret2 == 0) {		ret = OK;		_sensor_ok = true;		DEVICE_LOG("(%dm %dHz) with address %d found", (int)_max_distance,			   (int)(1e6f / _conversion_interval), SF1XX_BASEADDR);	}	return ret;}

intMB12XX::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(distance_sensor_s));	_index_counter = MB12XX_BASEADDR;	/* set temp sonar i2c address to base adress */	set_device_address(_index_counter);		/* set I2c port to temp sonar i2c adress */	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 = {};	_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,				 &_orb_class_instance, ORB_PRIO_LOW);	if (_distance_sensor_topic == nullptr) {		DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");	}	// XXX we should find out why we need to wait 200 ms here	usleep(200000);	/* check for connected rangefinders on each i2c port:	   We start from i2c base address (0x70 = 112) and count downwards	   So second iteration it uses i2c address 111, third iteration 110 and so on*/	for (unsigned counter = 0; counter <= MB12XX_MAX_RANGEFINDERS; counter++) {		_index_counter = MB12XX_BASEADDR - counter;	/* set temp sonar i2c address to base adress - counter */		set_device_address(_index_counter);			/* set I2c port to temp sonar i2c adress */		int ret2 = measure();		if (ret2 == 0) { /* sonar is present -> store address_index in array */			addr_ind.push_back(_index_counter);			DEVICE_DEBUG("sonar added");			_latest_sonar_measurements.push_back(200);		}	}	_index_counter = MB12XX_BASEADDR;	set_device_address(_index_counter); /* set i2c port back to base adress for rest of driver */	/* if only one sonar detected, no special timing is required between firing, so use default */	if (addr_ind.size() == 1) {		_cycling_rate = MB12XX_CONVERSION_INTERVAL;	} else {		_cycling_rate = TICKS_BETWEEN_SUCCESIVE_FIRES;	}	/* show the connected sonars in terminal */	for (unsigned i = 0; i < addr_ind.size(); i++) {		DEVICE_LOG("sonar %d with address %d added", (i + 1), addr_ind[i]);	}	DEVICE_DEBUG("Number of sonars connected: %d", addr_ind.size());	ret = OK;	/* sensor is ok, but we don't really know if it is within range */	_sensor_ok = true;	return ret;}

voidPX4FMU::task_main(){	/* force a reset of the update rate */	_current_update_rate = 0;	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));	_param_sub = orb_subscribe(ORB_ID(parameter_update));#ifdef HRT_PPM_CHANNEL	// rc input, published to ORB	struct rc_input_values rc_in;	orb_advert_t to_input_rc = 0;	memset(&rc_in, 0, sizeof(rc_in));	rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM;#endif	/* initialize PWM limit lib */	pwm_limit_init(&_pwm_limit);	update_pwm_rev_mask();	/* loop until killed */	while (!_task_should_exit) {		if (_groups_subscribed != _groups_required) {			subscribe();			_groups_subscribed = _groups_required;			/* force setting update rate */			_current_update_rate = 0;		}		/*		 * Adjust actuator topic update rate to keep up with		 * the highest servo update rate configured.		 *		 * We always mix at max rate; some channels may update slower.		 */		unsigned max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate;		if (_current_update_rate != max_rate) {			_current_update_rate = max_rate;			int update_rate_in_ms = int(1000 / _current_update_rate);			/* reject faster than 500 Hz updates */			if (update_rate_in_ms < 2) {				update_rate_in_ms = 2;			}			/* reject slower than 10 Hz updates */			if (update_rate_in_ms > 100) {				update_rate_in_ms = 100;			}			DEVICE_DEBUG("adjusted actuator update interval to %ums", update_rate_in_ms);			for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {				if (_control_subs[i] > 0) {					orb_set_interval(_control_subs[i], update_rate_in_ms);				}			}			// set to current max rate, even if we are actually checking slower/faster			_current_update_rate = max_rate;		}		/* sleep waiting for data, stopping to check for PPM		 * input at 50Hz */		int ret = ::poll(_poll_fds, _poll_fds_num, CONTROL_INPUT_DROP_LIMIT_MS);		/* this would be bad... */		if (ret < 0) {			DEVICE_LOG("poll error %d", errno);			continue;		} else if (ret == 0) {			/* timeout: no control data, switch to failsafe values *///			warnx("no PWM: failsafe");		} else {			/* get controls for required topics */			unsigned poll_id = 0;			for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {				if (_control_subs[i] > 0) {					if (_poll_fds[poll_id].revents & POLLIN) {						orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);					}					poll_id++;				}			}			/* can we mix? */			if (_mixers != nullptr) {				size_t num_outputs;				switch (_mode) {				case MODE_2PWM:					num_outputs = 2;					break;//.........这里部分代码省略.........

/** * 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) {			// Init PWM limits			pwm_limit_init(&_pwm_limit);			// Get arming state			_armed_sub = orb_subscribe(ORB_ID(actuator_armed));			/* Subscribe to actuator groups */			subscribe();			/* set the uorb update interval lower than the driver pwm interval */			for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; ++i) {				orb_set_interval(_control_subs[i], 1000.0f / PCA9685_PWMFREQ - 5);			}			_mode_on_initialized = true;		}		/* check if anything updated */		int ret = ::poll(_poll_fds, _poll_fds_num, 0);		if (ret < 0) {			DEVICE_LOG("poll error %d", errno);		} else if (ret == 0) {	//			warnx("no PWM: failsafe");		} else {			/* get controls for required topics */			unsigned poll_id = 0;			for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {				if (_control_subs[i] > 0) {					if (_poll_fds[poll_id].revents & POLLIN) {						orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);					}				}				poll_id++;			}			if (_mixers != nullptr) {				size_t num_outputs = actuator_outputs_s::NUM_ACTUATOR_OUTPUTS;				// do mixing				num_outputs = _mixers->mix(_outputs, num_outputs, NULL);				// disable unused ports by setting their output to NaN				for (size_t i = 0; i < sizeof(_outputs) / sizeof(_outputs[0]); i++) {					if (i >= num_outputs) {						_outputs[i] = NAN_VALUE;					}				}				// Finally, write servo values to motors				for (int i = 0; i < num_outputs; i++) {					uint16_t new_value = PCA9685_PWMCENTER +								 (_outputs[i] * M_PI_F * PCA9685_SCALE);					// DEVICE_DEBUG("%d: current: %u, new %u, control %.2f", i, _current_values[i], new_value,					//      (double)_controls[1].control[i]);					if (isfinite(new_value) &&						new_value >= PCA9685_PWMMIN &&						new_value <= PCA9685_PWMMAX) {						setPin(i, new_value);						_rates[i] = new_value;					}				}			}		}		bool updated;		// Update Arming state		orb_check(_armed_sub, &updated);		if (updated) {			orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);			bool set_armed = (_armed.armed || _armed.prearmed) && !_armed.lockdown;			if (_servo_armed != set_armed) {				_servo_armed = set_armed;			}		}		// Update AUX controls update		// orb_check(_actuator_controls_sub, &updated);//.........这里部分代码省略.........

intI2C::init(){	int ret = OK;	unsigned bus_index;	// attach to the i2c bus	_dev = px4_i2cbus_initialize(_bus);	if (_dev == nullptr) {		DEVICE_DEBUG("failed to init I2C");		ret = -ENOENT;		goto out;	}	// the above call fails for a non-existing bus index,	// so the index math here is safe.	bus_index = _bus - 1;	// abort if the max frequency we allow (the frequency we ask)	// is smaller than the bus frequency	if (_bus_clocks[bus_index] > _frequency) {		(void)px4_i2cbus_uninitialize(_dev);		_dev = nullptr;		DEVICE_LOG("FAIL: too slow for bus #%u: %u KHz, device max: %u KHz)",			   _bus, _bus_clocks[bus_index] / 1000, _frequency / 1000);		ret = -EINVAL;		goto out;	}	// set frequency for this instance once to the bus speed	// the bus speed is the maximum supported by all devices on the bus,	// as we have to prioritize performance over compatibility.	// If a new device requires a lower clock speed, this has to be	// manually set via "fmu i2c <bus> <clock>" before starting any	// drivers.	// This is necessary as automatically lowering the bus speed	// for maximum compatibility could induce timing issues on	// critical sensors the adopter might be unaware of.	// set the bus frequency on the first access if it has	// not been set yet	if (_bus_clocks[bus_index] == 0) {		_bus_clocks[bus_index] = _frequency;	}	// 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 device node, etc	ret = CDev::init();	if (ret != OK) {		DEVICE_DEBUG("cdev init failed");		goto out;	}	// tell the world where we are	DEVICE_LOG("on I2C bus %d at 0x%02x (bus: %u KHz, max: %u KHz)",		   _bus, _address, _bus_clocks[bus_index] / 1000, _frequency / 1000);out:	if ((ret != OK) && (_dev != nullptr)) {		px4_i2cbus_uninitialize(_dev);		_dev = nullptr;	}	return ret;}

intSF0X::init(){	int hw_model;	param_get(param_find("SENS_EN_SF0X"), &hw_model);	switch (hw_model) {	case 0:		DEVICE_LOG("disabled.");		return 0;	case 1: /* SF02 (40m, 12 Hz)*/		_min_distance = 0.3f;		_max_distance = 40.0f;		_conversion_interval =	83334;		break;	case 2:  /* SF10/a (25m 32Hz) */		_min_distance = 0.01f;		_max_distance = 25.0f;		_conversion_interval = 31250;		break;	case 3:  /* SF10/b (50m 32Hz) */		_min_distance = 0.01f;		_max_distance = 50.0f;		_conversion_interval = 31250;		break;	case 4:  /* SF10/c (100m 16Hz) */		_min_distance = 0.01f;		_max_distance = 100.0f;		_conversion_interval = 62500;		break;	case 5:		/* SF11/c (120m 20Hz) */		_min_distance = 0.01f;		_max_distance = 120.0f;		_conversion_interval = 50000;		break;	default:		DEVICE_LOG("invalid HW model %d.", hw_model);		return -1;	}	/* status */	int ret = 0;	do { /* create a scope to handle exit conditions using break */		/* do regular cdev init */		ret = CDev::init();		if (ret != OK) { break; }		/* allocate basic report buffers */		_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));		if (_reports == nullptr) {			warnx("mem err");			ret = -1;			break;		}		_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 = {};		_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,					 &_orb_class_instance, ORB_PRIO_HIGH);		if (_distance_sensor_topic == nullptr) {			DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");		}	} while (0);	/* close the fd */	::close(_fd);	_fd = -1;	return OK;}

//.........这里部分代码省略.........		case CheckFW:			_fw_server_status = request_fw_check();			break;		case None:		default:			break;		}		// update actuator controls subscriptions if needed		if (_groups_subscribed != _groups_required) {			subscribe();			_groups_subscribed = _groups_required;		}		// Mutex is unlocked while the thread is blocked on IO multiplexing		(void)pthread_mutex_unlock(&_node_mutex);		perf_end(_perfcnt_esc_mixer_total_elapsed); // end goes first, it's not a mistake		const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs);		perf_begin(_perfcnt_esc_mixer_total_elapsed);		(void)pthread_mutex_lock(&_node_mutex);		node_spin_once();  // Non-blocking		bool new_output = false;		// this would be bad...		if (poll_ret < 0) {			DEVICE_LOG("poll error %d", errno);			continue;		} else {			// get controls for required topics			bool controls_updated = false;			for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {				if (_control_subs[i] > 0) {					if (_poll_fds[_poll_ids[i]].revents & POLLIN) {						controls_updated = true;						orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);					}				}			}			/*			  see if we have any direct actuator updates			 */			if (_actuator_direct_sub != -1 &&			    (_poll_fds[_actuator_direct_poll_fd_num].revents & POLLIN) &&			    orb_copy(ORB_ID(actuator_direct), _actuator_direct_sub, &_actuator_direct) == OK &&			    !_test_in_progress) {				if (_actuator_direct.nvalues > actuator_outputs_s::NUM_ACTUATOR_OUTPUTS) {					_actuator_direct.nvalues = actuator_outputs_s::NUM_ACTUATOR_OUTPUTS;				}				memcpy(&_outputs.output[0], &_actuator_direct.values[0],				       _actuator_direct.nvalues * sizeof(float));				_outputs.noutputs = _actuator_direct.nvalues;				new_output = true;			}

voidSF0X::cycle(){	/* fds initialized? */	if (_fd < 0) {		/* open fd */		_fd = ::open(_port, O_RDWR | O_NOCTTY | O_NONBLOCK);	}	/* collection phase? */	if (_collect_phase) {		/* perform collection */		int collect_ret = collect();		if (collect_ret == -EAGAIN) {			/* reschedule to grab the missing bits, time to transmit 8 bytes @ 9600 bps */			work_queue(HPWORK,				   &_work,				   (worker_t)&SF0X::cycle_trampoline,				   this,				   USEC2TICK(1042 * 8));			return;		}		if (OK != collect_ret) {			/* we know the sensor needs about four seconds to initialize */			if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) {				DEVICE_LOG("collection error #%u", _consecutive_fail_count);			}			_consecutive_fail_count++;			/* restart the measurement state machine */			start();			return;		} else {			/* apparently success */			_consecutive_fail_count = 0;		}		/* 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)&SF0X::cycle_trampoline,				   this,				   _measure_ticks - USEC2TICK(_conversion_interval));			return;		}	}	/* measurement phase */	if (OK != measure()) {		DEVICE_LOG("measure error");	}	/* next phase is collection */	_collect_phase = true;	/* schedule a fresh cycle call when the measurement is done */	work_queue(HPWORK,		   &_work,		   (worker_t)&SF0X::cycle_trampoline,		   this,		   USEC2TICK(_conversion_interval));}