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

bool VtolType::init(){	const char *dev = PWM_OUTPUT0_DEVICE_PATH;	int fd = px4_open(dev, 0);	if (fd < 0) {		PX4_ERR("can't open %s", dev);		return false;	}	int ret = px4_ioctl(fd, PWM_SERVO_GET_MAX_PWM, (long unsigned int)&_max_mc_pwm_values);	if (ret != PX4_OK) {		PX4_ERR("failed getting max values");		px4_close(fd);		return false;	}	ret = px4_ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, (long unsigned int)&_disarmed_pwm_values);	if (ret != PX4_OK) {		PX4_ERR("failed getting disarmed values");		px4_close(fd);		return false;	}	return true;}

/** * @brief Resets the driver. */voidreset(){	if (!instance) {		PX4_WARN("No ll40ls driver running");		return;	}	int fd = px4_open(instance->get_dev_name(), O_RDONLY);	if (fd < 0) {		PX4_ERR("Error opening fd");		return;	}	if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) {		PX4_ERR("driver reset failed");		px4_close(fd);		return;	}	if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {		PX4_ERR("driver poll restart failed");		px4_close(fd);		return;	}}

void led_deinit(){	if (leds >= 0) {		px4_close(leds);	}	if (rgbleds >= 0) {		px4_close(rgbleds);	}}

int uORB::Manager::node_advertise(    const struct orb_metadata *meta,    int *instance,    int priority){  int fd = -1;  int ret = ERROR;  /* fill advertiser data */  const struct orb_advertdata adv = { meta, instance, priority };  /* open the control device */  fd = px4_open(TOPIC_MASTER_DEVICE_PATH, 0);  if (fd < 0)    goto out;  /* advertise the object */  ret = px4_ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)&adv);  /* it's PX4_OK if it already exists */  if ((PX4_OK != ret) && (EEXIST == errno)) {    ret = PX4_OK;  }out:  if (fd >= 0)    px4_close(fd);  return ret;}

/*** Adjust idle speed for fw mode.*/void VtolType::set_idle_fw(){	const char *dev = PWM_OUTPUT0_DEVICE_PATH;	int fd = px4_open(dev, 0);	if (fd < 0) {		PX4_WARN("can't open %s", dev);	}	struct pwm_output_values pwm_values;	memset(&pwm_values, 0, sizeof(pwm_values));	for (int i = 0; i < _params->vtol_motor_count; i++) {		pwm_values.values[i] = PWM_MOTOR_OFF;		pwm_values.channel_count++;	}	int ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);	if (ret != OK) {		PX4_WARN("failed setting min values");	}	px4_close(fd);}

orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority){  int result, fd;  orb_advert_t advertiser;  //warnx("orb_advertise_multi meta = %p/n", meta);  /* open the node as an advertiser */  fd = node_open(PUBSUB, meta, data, true, instance, priority);  if (fd == ERROR) {    warnx("node_open as advertiser failed.");    return nullptr;  }  /* get the advertiser handle and close the node */  result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);  px4_close(fd);  if (result == ERROR) {    warnx("px4_ioctl ORBIOCGADVERTISER  failed. fd = %d", fd);    return nullptr;  }  /* the advertiser must perform an initial publish to initialise the object */  result = orb_publish(meta, advertiser, data);  if (result == ERROR) {    warnx("orb_publish failed");    return nullptr;  }  return advertiser;}

int test_gpio(int argc, char *argv[]){	int		ret = 0;#ifdef PX4IO_DEVICE_PATH	int fd = px4_open(PX4IO_DEVICE_PATH, 0);	if (fd < 0) {		printf("GPIO: open fail/n");		return ERROR;	}	/* set all GPIOs to default state */	px4_ioctl(fd, GPIO_RESET, ~0);	/* XXX need to add some GPIO waving stuff here */	/* Go back to default */	px4_ioctl(fd, GPIO_RESET, ~0);	px4_close(fd);	printf("/t GPIO test successful./n");#endif	return ret;}

static bool gnssCheck(orb_advert_t *mavlink_log_pub, bool report_fail){	bool success = true;	int gpsSub = orb_subscribe(ORB_ID(vehicle_gps_position));	//Wait up to 2000ms to allow the driver to detect a GNSS receiver module	px4_pollfd_struct_t fds[1];	fds[0].fd = gpsSub;	fds[0].events = POLLIN;	if(px4_poll(fds, 1, 2000) <= 0) {		success = false;	}	else {		struct vehicle_gps_position_s gps;		if ( (OK != orb_copy(ORB_ID(vehicle_gps_position), gpsSub, &gps)) ||		    (hrt_elapsed_time(&gps.timestamp_position) > 1000000)) {			success = false;		}	}	//Report failure to detect module	if (!success) {		if (report_fail) {			mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS RECEIVER MISSING");		}	}	px4_close(gpsSub);	return success;}

/*** Disable all multirotor motors when in fw mode.*/voidStandard::set_max_mc(unsigned pwm_value){	int ret;	unsigned servo_count;	const char *dev = PWM_OUTPUT0_DEVICE_PATH;	int fd = px4_open(dev, 0);	if (fd < 0) {		PX4_WARN("can't open %s", dev);	}	ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);	struct pwm_output_values pwm_values;	memset(&pwm_values, 0, sizeof(pwm_values));	for (int i = 0; i < _params->vtol_motor_count; i++) {		pwm_values.values[i] = pwm_value;		pwm_values.channel_count = _params->vtol_motor_count;	}	ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);	if (ret != OK) {		PX4_WARN("failed setting max values");	}	px4_close(fd);}

/** * Start the driver on a specific bus. * * This function only returns if the sensor is up and running * or could not be detected successfully. */intstart_bus(uint8_t rotation, int i2c_bus){	int fd = -1;	if (g_dev != nullptr) {		PX4_ERR("already started");		return PX4_ERROR;	}	/* create the driver */	g_dev = new SF1XX(rotation, i2c_bus);	if (g_dev == nullptr) {		goto fail;	}	if (OK != g_dev->init()) {		goto fail;	}	/* set the poll rate to default, starts automatic data collection */	fd = px4_open(SF1XX_DEVICE_PATH, O_RDONLY);	if (fd < 0) {		goto fail;	}	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {		px4_close(fd);		goto fail;	}	px4_close(fd);	return PX4_OK;fail:	if (g_dev != nullptr) {		delete g_dev;		g_dev = nullptr;	}	return PX4_ERROR;}

// 1M 8N1 serial connection to NRF51intSyslink::open_serial(const char *dev){#ifndef B1000000#define B1000000 1000000#endif	int rate = B1000000;	// open uart	int fd = px4_open(dev, O_RDWR | O_NOCTTY);	int termios_state = -1;	if (fd < 0) {		PX4_ERR("failed to open uart device!");		return -1;	}	// set baud rate	struct termios config;	tcgetattr(fd, &config);	// clear ONLCR flag (which appends a CR for every LF)	config.c_oflag = 0;	config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);	// Disable hardware flow control	config.c_cflag &= ~CRTSCTS;	/* Set baud rate */	if (cfsetispeed(&config, rate) < 0 || cfsetospeed(&config, rate) < 0) {		warnx("ERR SET BAUD %s: %d/n", dev, termios_state);		px4_close(fd);		return -1;	}	if ((termios_state = tcsetattr(fd, TCSANOW, &config)) < 0) {		PX4_WARN("ERR SET CONF %s/n", dev);		px4_close(fd);		return -1;	}	return fd;}

int test_tone(int argc, char *argv[]){	int fd, result;	unsigned long tone;	fd = px4_open(TONEALARM0_DEVICE_PATH, O_WRONLY);	if (fd < 0) {		printf("failed opening " TONEALARM0_DEVICE_PATH "/n");		goto out;	}	tone = 1;	if (argc == 2) {		tone = atoi(argv[1]);	}	if (tone  == 0) {		result = px4_ioctl(fd, TONE_SET_ALARM, TONE_STOP_TUNE);		if (result < 0) {			printf("failed clearing alarms/n");			goto out;		} else {			printf("Alarm stopped./n");		}	} else {		result = px4_ioctl(fd, TONE_SET_ALARM, TONE_STOP_TUNE);		if (result < 0) {			printf("failed clearing alarms/n");			goto out;		}		result = px4_ioctl(fd, TONE_SET_ALARM, tone);		if (result < 0) {			printf("failed setting alarm %lu/n", tone);		} else {			printf("Alarm %lu (disable with: tests tone 0)/n", tone);		}	}out:	if (fd >= 0) {		px4_close(fd);	}	return 0;}

int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance){	/*	 * Generate the path to the node and try to open it.	 */	char path[orb_maxpath];	int inst = instance;	int ret = uORB::Utils::node_mkpath(path, meta, &inst);	if (ret != OK) {		errno = -ret;		return PX4_ERROR;	}#if defined(__PX4_NUTTX)	struct stat buffer;	ret = stat(path, &buffer);#else	ret = px4_access(path, F_OK);#ifdef ORB_COMMUNICATOR	if (ret == -1 && meta != nullptr && !_remote_topics.empty()) {		ret = (_remote_topics.find(meta->o_name) != _remote_topics.end()) ? OK : PX4_ERROR;	}#endif /* ORB_COMMUNICATOR */#endif	if (ret == 0) {		// we know the topic exists, but it's not necessarily advertised/published yet (for example		// if there is only a subscriber)		// The open() will not lead to memory allocations.		int fd = px4_open(path, 0);		if (fd >= 0) {			unsigned long is_published;			if (px4_ioctl(fd, ORBIOCISPUBLISHED, (unsigned long)&is_published) == 0) {				if (!is_published) {					ret = PX4_ERROR;				}			}			px4_close(fd);		}	}	return ret;}

void Tiltrotor::set_rear_motor_state(rear_motor_state state){	int pwm_value = PWM_DEFAULT_MAX;	// map desired rear rotor state to max allowed pwm signal	switch (state) {	case ENABLED:		pwm_value = PWM_DEFAULT_MAX;		_rear_motors = ENABLED;		break;	case DISABLED:		pwm_value = PWM_LOWEST_MAX;		_rear_motors = DISABLED;		break;	case IDLE:		pwm_value = _params->idle_pwm_mc;		_rear_motors = IDLE;		break;	}	int ret;	unsigned servo_count;	char *dev = PWM_OUTPUT0_DEVICE_PATH;	int fd = px4_open(dev, 0);	if (fd < 0) {PX4_WARN("can't open %s", dev);}	ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);	struct pwm_output_values pwm_values;	memset(&pwm_values, 0, sizeof(pwm_values));	for (int i = 0; i < _params->vtol_motor_count; i++) {		if (is_motor_off_channel(i)) {			pwm_values.values[i] = pwm_value;		} else {			pwm_values.values[i] = PWM_DEFAULT_MAX;		}		pwm_values.channel_count = _params->vtol_motor_count;	}	ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);	if (ret != OK) {PX4_WARN("failed setting max values");}	px4_close(fd);}

static intaccel(int argc, char *argv[], const char *path){	printf("/tACCEL: test start/n");	fflush(stdout);	int		fd;	struct accel_report buf;	int		ret;	fd = px4_open(path, O_RDONLY);	if (fd < 0) {		printf("/tACCEL: open fail, run <mpu6000 start> or <lsm303 start> or <bma180 start> first./n");		return ERROR;	}	/* wait at least 100ms, sensor should have data after no more than 20ms */	usleep(100000);	/* read data - expect samples */	ret = px4_read(fd, &buf, sizeof(buf));	if (ret != sizeof(buf)) {		printf("/tACCEL: read1 fail (%d)/n", ret);		return ERROR;	} else {		printf("/tACCEL accel: x:%8.4f/ty:%8.4f/tz:%8.4f m/s^2/n", (double)buf.x, (double)buf.y, (double)buf.z);	}	if (fabsf(buf.x) > 30.0f || fabsf(buf.y) > 30.0f || fabsf(buf.z) > 30.0f) {		warnx("ACCEL acceleration values out of range!");		return ERROR;	}	float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z);	if (len < 8.0f || len > 12.0f) {		warnx("ACCEL scale error!");		return ERROR;	}	/* Let user know everything is ok */	printf("/tOK: ACCEL passed all tests successfully/n");	px4_close(fd);	return OK;}

static intgyro(int argc, char *argv[], const char *path){	printf("/tGYRO: test start/n");	fflush(stdout);	int		fd;	struct gyro_report buf;	int		ret;	fd = px4_open(path, O_RDONLY);	if (fd < 0) {		printf("/tGYRO: open fail, run <l3gd20 start> or <mpu6000 start> first./n");		return ERROR;	}	/* wait at least 5 ms, sensor should have data after that */	usleep(5000);	/* read data - expect samples */	ret = px4_read(fd, &buf, sizeof(buf));	if (ret != sizeof(buf)) {		printf("/tGYRO: read fail (%d)/n", ret);		return ERROR;	} else {		printf("/tGYRO rates: x:%8.4f/ty:%8.4f/tz:%8.4f rad/s/n", (double)buf.x, (double)buf.y, (double)buf.z);	}	float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z);	if (len > 0.3f) {		warnx("GYRO scale error!");		return ERROR;	}	/* Let user know everything is ok */	printf("/tOK: GYRO passed all tests successfully/n");	px4_close(fd);	return OK;}

static intmag(int argc, char *argv[], const char *path){	printf("/tMAG: test start/n");	fflush(stdout);	int		fd;	struct mag_report buf;	int		ret;	fd = px4_open(path, O_RDONLY);	if (fd < 0) {		printf("/tMAG: open fail, run <hmc5883 start> or <lsm303 start> first./n");		return ERROR;	}	/* wait at least 5 ms, sensor should have data after that */	usleep(5000);	/* read data - expect samples */	ret = px4_read(fd, &buf, sizeof(buf));	if (ret != sizeof(buf)) {		printf("/tMAG: read fail (%d)/n", ret);		return ERROR;	} else {		printf("/tMAG values: x:%8.4f/ty:%8.4f/tz:%8.4f/n", (double)buf.x, (double)buf.y, (double)buf.z);	}	float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z);	if (len < 0.25f || len > 3.0f) {		warnx("MAG scale error!");		return ERROR;	}	/* Let user know everything is ok */	printf("/tOK: MAG passed all tests successfully/n");	px4_close(fd);	return OK;}

static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional){	bool success = true;	char s[30];	sprintf(s, "%s%u", MAG_BASE_DEVICE_PATH, instance);	int fd = px4_open(s, 0);	if (fd < 0) {		if (!optional) {			mavlink_and_console_log_critical(mavlink_fd,							 "PREFLIGHT FAIL: NO MAG SENSOR #%u", instance);		}		return false;	}	int calibration_devid;	int ret;	int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0);	sprintf(s, "CAL_MAG%u_ID", instance);	param_get(param_find(s), &(calibration_devid));	if (devid != calibration_devid) {		mavlink_and_console_log_critical(mavlink_fd,						 "PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance);		success = false;		goto out;	}	ret = px4_ioctl(fd, MAGIOCSELFTEST, 0);	if (ret != OK) {		mavlink_and_console_log_critical(mavlink_fd,						 "PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance);		success = false;		goto out;	}out:	px4_close(fd);	return success;}

static bool baroCheck(int mavlink_fd, unsigned instance, bool optional){	bool success = true;	char s[30];	sprintf(s, "%s%u", BARO_BASE_DEVICE_PATH, instance);	int fd = px4_open(s, 0);	if (fd < 0) {		if (!optional) {			mavlink_and_console_log_critical(mavlink_fd,							 "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance);		}		return false;	}	px4_close(fd);	return success;}

static intdo_import(const char *param_file_name){	int fd = px4_open(param_file_name, O_RDONLY);	if (fd < 0) {		warn("open '%s'", param_file_name);		return 1;	}	int result = param_import(fd);	px4_close(fd);	if (result < 0) {		warnx("error importing from '%s'", param_file_name);		return 1;	}	return 0;}

static intdo_save(const char *param_file_name){	/* create the file */	int fd = px4_open(param_file_name, O_WRONLY | O_CREAT, 0x777);	if (fd < 0) {		warn("opening '%s' failed", param_file_name);		return 1;	}	int result = param_export(fd, false);	px4_close(fd);	if (result < 0) {		(void)unlink(param_file_name);		warnx("error exporting to '%s'", param_file_name);		return 1;	}	return 0;}

int initialise_uart(){	// open uart	_uart_fd = px4_open(_device, O_RDWR | O_NOCTTY);	int termios_state = -1;	if (_uart_fd < 0) {		PX4_ERR("failed to open uart device!");		return -1;	}	// set baud rate	int speed = B921600;	struct termios uart_config;	tcgetattr(_uart_fd, &uart_config);	// clear ONLCR flag (which appends a CR for every LF)	uart_config.c_oflag &= ~ONLCR;	/* Set baud rate */	if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {		warnx("ERR SET BAUD %s: %d/n", _device, termios_state);		::close(_uart_fd);		return -1;	}	if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) {		PX4_WARN("ERR SET CONF %s/n", _device);		px4_close(_uart_fd);		return -1;	}	/* setup output flow control */	if (enable_flow_control(false)) {		PX4_WARN("hardware flow disable failed");	}	return _uart_fd;}

static intbaro(int argc, char *argv[], const char *path){	printf("/tBARO: test start/n");	fflush(stdout);	int		fd;	struct baro_report buf;	int		ret;	fd = px4_open(path, O_RDONLY);	if (fd < 0) {		printf("/tBARO: open fail, run <ms5611 start> or <lps331 start> first./n");		return ERROR;	}	/* wait at least 5 ms, sensor should have data after that */	usleep(5000);	/* read data - expect samples */	ret = px4_read(fd, &buf, sizeof(buf));	if (ret != sizeof(buf)) {		printf("/tBARO: read fail (%d)/n", ret);		return ERROR;	} else {		printf("/tBARO pressure: %8.4f mbar/talt: %8.4f m/ttemp: %8.4f deg C/n", (double)buf.pressure, (double)buf.altitude,		       (double)buf.temperature);	}	/* Let user know everything is ok */	printf("/tOK: BARO passed all tests successfully/n");	px4_close(fd);	return OK;}

int test_adc(int argc, char *argv[]){	int fd = px4_open(ADC0_DEVICE_PATH, O_RDONLY);	if (fd < 0) {		PX4_ERR("ERROR: can't open ADC device");		return 1;	}	for (unsigned i = 0; i < 5; i++) {		/* make space for a maximum of twelve channels */		struct adc_msg_s data[12];		/* read all channels available */		ssize_t count = px4_read(fd, data, sizeof(data));		if (count < 0) {			goto errout_with_dev;		}		unsigned channels = count / sizeof(data[0]);		for (unsigned j = 0; j < channels; j++) {			printf("%d: %u  ", data[j].am_channel, data[j].am_data);		}		printf("/n");		usleep(150000);	}	printf("/t ADC test successful./n");errout_with_dev:	if (fd != 0) { px4_close(fd); }	return OK;}

static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool report_fail){	bool success = true;	int ret;	int fd = orb_subscribe(ORB_ID(airspeed));	struct airspeed_s airspeed;	if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) ||	    (hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) {		if (report_fail) {			mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");		}		success = false;		goto out;	}	if (fabsf(airspeed.confidence) < 0.99f) {		if (report_fail) {			mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR COMM ERROR");		}		success = false;		goto out;	}	if (fabsf(airspeed.indicated_airspeed_m_s) > 6.0f) {		if (report_fail) {			mavlink_and_console_log_critical(mavlink_log_pub, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");		}		// XXX do not make this fatal yet	}out:	px4_close(fd);	return success;}

/** * Reset the driver. */intreset(){	int fd = px4_open(SF1XX_DEVICE_PATH, O_RDONLY);	if (fd < 0) {		PX4_ERR("failed");		return PX4_ERROR;	}	if (ioctl(fd, SENSORIOCRESET, 0) < 0) {		PX4_ERR("driver reset failed");		return PX4_ERROR;	}	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {		PX4_ERR("driver poll restart failed");		return PX4_ERROR;	}	px4_close(fd);	return PX4_OK;}

int do_mag_calibration(int mavlink_fd){	mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);	struct mag_scale mscale_null = {		0.0f,		1.0f,		0.0f,		1.0f,		0.0f,		1.0f,	};	int result = OK;		// Determine which mags are available and reset each	int32_t	device_ids[max_mags];	char str[30];	for (size_t i=0; i<max_mags; i++) {		device_ids[i] = 0; // signals no mag	}		for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) {		// Reset mag id to mag not available		(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);		result = param_set_no_notification(param_find(str), &(device_ids[cur_mag]));		if (result != OK) {			mavlink_and_console_log_info(mavlink_fd, "[cal] Unable to reset CAL_MAG%u_ID", cur_mag);			break;		}		// Attempt to open mag		(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);		int fd = px4_open(str, O_RDONLY);		if (fd < 0) {			continue;		}		// Get device id for this mag		device_ids[cur_mag] = px4_ioctl(fd, DEVIOCGDEVICEID, 0);		// Reset mag scale		result = px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);		if (result != OK) {			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, cur_mag);		}		/* calibrate range */		if (result == OK) {			result = px4_ioctl(fd, MAGIOCCALIBRATE, fd);			if (result != OK) {				mavlink_and_console_log_info(mavlink_fd, "[cal] Skipped scale calibration, sensor %u", cur_mag);				/* this is non-fatal - mark it accordingly */				result = OK;			}		}		px4_close(fd);	}	// Calibrate all mags at the same time	if (result == OK) {		switch (mag_calibrate_all(mavlink_fd, device_ids)) {			case calibrate_return_cancelled:				// Cancel message already displayed, we're done here				result = ERROR;				break;							case calibrate_return_ok:				/* auto-save to EEPROM */				result = param_save_default();				/* if there is a any preflight-check system response, let the barrage of messages through */				usleep(200000);				if (result == OK) {					mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);					mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);					break;				} else {					mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);				}				// Fall through							default:				mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);				break;		}	}	/* give this message enough time to propagate */	usleep(600000);		return result;}

calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]){	calibrate_return result = calibrate_return_ok;	mag_worker_data_t worker_data;		worker_data.mavlink_fd = mavlink_fd;	worker_data.done_count = 0;	worker_data.calibration_points_perside = 40;	worker_data.calibration_interval_perside_seconds = 20;	worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000;	// Collect: Right-side up, Left Side, Nose down	worker_data.side_data_collected[DETECT_ORIENTATION_RIGHTSIDE_UP] = false;	worker_data.side_data_collected[DETECT_ORIENTATION_LEFT] = false;	worker_data.side_data_collected[DETECT_ORIENTATION_NOSE_DOWN] = false;	worker_data.side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = false;	worker_data.side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = false;	worker_data.side_data_collected[DETECT_ORIENTATION_RIGHT] = false;		for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {		// Initialize to no subscription		worker_data.sub_mag[cur_mag] = -1;				// Initialize to no memory allocated		worker_data.x[cur_mag] = NULL;		worker_data.y[cur_mag] = NULL;		worker_data.z[cur_mag] = NULL;		worker_data.calibration_counter_total[cur_mag] = 0;	}	const unsigned int calibration_points_maxcount = calibration_sides * worker_data.calibration_points_perside;		char str[30];		for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {		worker_data.x[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));		worker_data.y[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));		worker_data.z[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));		if (worker_data.x[cur_mag] == NULL || worker_data.y[cur_mag] == NULL || worker_data.z[cur_mag] == NULL) {			mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: out of memory");			result = calibrate_return_error;		}	}		// Setup subscriptions to mag sensors	if (result == calibrate_return_ok) {		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {			if (device_ids[cur_mag] != 0) {				// Mag in this slot is available				worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), cur_mag);				if (worker_data.sub_mag[cur_mag] < 0) {					mavlink_and_console_log_critical(mavlink_fd, "[cal] Mag #%u not found, abort", cur_mag);					result = calibrate_return_error;					break;				}			}		}	}		// Limit update rate to get equally spaced measurements over time (in ms)	if (result == calibrate_return_ok) {		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {			if (device_ids[cur_mag] != 0) {				// Mag in this slot is available				unsigned int orb_interval_msecs = (worker_data.calibration_interval_perside_useconds / 1000) / worker_data.calibration_points_perside;								//mavlink_and_console_log_info(mavlink_fd, "Orb interval %u msecs", orb_interval_msecs);				orb_set_interval(worker_data.sub_mag[cur_mag], orb_interval_msecs);			}		}			}    	if (result == calibrate_return_ok) {		int cancel_sub  = calibrate_cancel_subscribe();		result = calibrate_from_orientation(mavlink_fd,                         // Mavlink fd to write output						    cancel_sub,                         // Subscription to vehicle_command for cancel support						    worker_data.side_data_collected,    // Sides to calibrate						    mag_calibration_worker,             // Calibration worker						    &worker_data,			// Opaque data for calibration worked						    true);				// true: lenient still detection		calibrate_cancel_unsubscribe(cancel_sub);	}		// Close subscriptions	for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {		if (worker_data.sub_mag[cur_mag] >= 0) {			px4_close(worker_data.sub_mag[cur_mag]);		}	}		// Calculate calibration values for each mag			float sphere_x[max_mags];	float sphere_y[max_mags];	float sphere_z[max_mags];//.........这里部分代码省略.........