static void usage()
{
	PX4_INFO("usage: navigator {start|stop|status|fencefile}");
}

void OutputMavlink::print_status()
{
	PX4_INFO("Output: Mavlink");
}

/**
 * Perform some basic functional tests on the driver;
 * make sure we can collect data from the sensor in polled
 * and automatic modes.
 */
void
test(enum IST8310_BUS busid)
{
	struct ist8310_bus_option &bus = find_bus(busid);
	struct mag_report report;
	ssize_t sz;
	int ret;
	const char *path = bus.devpath;

	int fd = open(path, O_RDONLY);

	if (fd < 0) {
		err(1, "%s open failed (try 'ist8310 start')", path);
	}

	/* do a simple demand read */
	sz = read(fd, &report, sizeof(report));

	if (sz != sizeof(report)) {
		err(1, "immediate read failed");
	}

	print_message(report);

	/* check if mag is onboard or external */
	if ((ret = ioctl(fd, MAGIOCGEXTERNAL, 0)) < 0) {
		errx(1, "failed to get if mag is onboard or external");
	}

	/* set the queue depth to 5 */
	if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) {
		errx(1, "failed to set queue depth");
	}

	/* start the sensor polling at 2Hz */
	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
		errx(1, "failed to set 2Hz poll rate");
	}

	/* read the sensor 5x and report each value */
	for (unsigned i = 0; i < 5; i++) {
		struct pollfd fds;

		/* wait for data to be ready */
		fds.fd = fd;
		fds.events = POLLIN;
		ret = poll(&fds, 1, 2000);

		if (ret != 1) {
			errx(1, "timed out waiting for sensor data");
		}

		/* now go get it */
		sz = read(fd, &report, sizeof(report));

		if (sz != sizeof(report)) {
			err(1, "periodic read failed");
		}

		print_message(report);
	}

	PX4_INFO("PASS");
	exit(0);
}

/**
 * @brief Starts the driver.
 */
void start(enum LL40LS_BUS busid, uint8_t rotation)
{
	int fd, ret;

	if (instance) {
		PX4_INFO("driver already started");
		return;
	}

	if (busid == LL40LS_BUS_PWM) {
		instance = new LidarLitePWM(LL40LS_DEVICE_PATH_PWM, rotation);

		if (!instance) {
			PX4_ERR("Failed to instantiate LidarLitePWM");
			return;
		}

		if (instance->init() != PX4_OK) {
			PX4_ERR("failed to initialize LidarLitePWM");
			stop();
			return;
		}

	} else {
		for (uint8_t i = 0; i < (sizeof(bus_options) / sizeof(bus_options[0])); i++) {
			if (busid != LL40LS_BUS_I2C_ALL && busid != bus_options[i].busid) {
				continue;
			}

			instance = new LidarLiteI2C(bus_options[i].busnum, bus_options[i].devname, rotation);

			if (!instance) {
				PX4_ERR("Failed to instantiate LidarLiteI2C");
				return;
			}

			if (instance->init() == PX4_OK) {
				break;
			}

			PX4_ERR("failed to initialize LidarLiteI2C on busnum=%u", bus_options[i].busnum);
			stop();
		}
	}

	if (!instance) {
		PX4_WARN("No LidarLite found");
		return;
	}

	fd = px4_open(instance->get_dev_name(), O_RDONLY);

	if (fd == -1) {
		PX4_ERR("Error opening fd");
		stop();
		return;
	}

	ret = px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
	px4_close(fd);

	if (ret < 0) {
		PX4_ERR("pollrate fail");
		stop();
		return;
	}
}

int navio_sysfs_rc_in_main(int argc, char *argv[])
{
	if (argc < 2) {
		usage("missing command");
		return 1;
	}

	if (!strcmp(argv[1], "start")) {

		if (rc_input != nullptr && rc_input->isRunning()) {
			PX4_WARN("already running");
			/* this is not an error */
			return 0;
		}

		rc_input = new RcInput();

		// Check if alloc worked.
		if (rc_input == nullptr) {
			PX4_ERR("alloc failed");
			return -1;
		}

		int ret = rc_input->start();

		if (ret != 0) {
			PX4_ERR("start failed");
		}

		return 0;
	}

	if (!strcmp(argv[1], "stop")) {

		if (rc_input == nullptr || !rc_input->isRunning()) {
			PX4_WARN("not running");
			/* this is not an error */
			return 0;
		}

		rc_input->stop();

		// Wait for task to die
		int i = 0;

		do {
			/* wait up to 3s */
			usleep(100000);

		} while (rc_input->isRunning() && ++i < 30);

		delete rc_input;
		rc_input = nullptr;

		return 0;
	}

	if (!strcmp(argv[1], "status")) {
		if (rc_input != nullptr && rc_input->isRunning()) {
			PX4_INFO("running");

		} else {
			PX4_INFO("not running\n");
		}

		return 0;
	}

	usage("unrecognized command");
	return 1;

}

int
sf0x_main(int argc, char *argv[])
{
	// check for optional arguments
	int ch;
	uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
	int myoptind = 1;
	const char *myoptarg = NULL;


	while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
		switch (ch) {
		case 'R':
			rotation = (uint8_t)atoi(myoptarg);
			PX4_INFO("Setting distance sensor orientation to %d", (int)rotation);
			break;

		default:
			PX4_WARN("Unknown option!");
		}
	}

	/*
	 * Start/load the driver.
	 */
	if (!strcmp(argv[myoptind], "start")) {
		if (argc > myoptind + 1) {
			sf0x::start(argv[myoptind + 1], rotation);

		} else {
			sf0x::start(SF0X_DEFAULT_PORT, rotation);
		}
	}

	/*
	 * Stop the driver
	 */
	if (!strcmp(argv[myoptind], "stop")) {
		sf0x::stop();
	}

	/*
	 * Test the driver/device.
	 */
	if (!strcmp(argv[myoptind], "test")) {
		sf0x::test();
	}

	/*
	 * Reset the driver.
	 */
	if (!strcmp(argv[myoptind], "reset")) {
		sf0x::reset();
	}

	/*
	 * Print driver information.
	 */
	if (!strcmp(argv[myoptind], "info") || !strcmp(argv[1], "status")) {
		sf0x::info();
	}

	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
}

int ver_main(int argc, char *argv[])
{
	/* defaults to an error */
	int ret = 1;

	/* first check if there are at least 2 params */
	if (argc >= 2) {
		if (argv[1] != NULL) {

			if (!strncmp(argv[1], sz_ver_hwcmp_str, sizeof(sz_ver_hwcmp_str))) {
				if (argc >= 3 && argv[2] != NULL) {
					/* compare 3rd parameter with HW_ARCH string, in case of match, return 0 */
					ret = strncmp(HW_ARCH, argv[2], strlen(HW_ARCH));

					if (ret == 0) {
						PX4_INFO("match: %s", HW_ARCH);
					}

					return ret;

				} else {
					warn("Not enough arguments, try 'ver hwcmp PX4FMU_V2'");
					return 1;
				}
			}

			/* check if we want to show all */
			bool show_all = !strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str));

			if (show_all || !strncmp(argv[1], sz_ver_hw_str, sizeof(sz_ver_hw_str))) {
				printf("HW arch: %s\n", HW_ARCH);
				ret = 0;

			}

			if (show_all || !strncmp(argv[1], sz_ver_git_str, sizeof(sz_ver_git_str))) {
				printf("FW git-hash: %s\n", px4_git_version);
				unsigned fwver = version_tag_to_number(px4_git_tag);
				unsigned major = (fwver >> (8 * 3)) & 0xFF;
				unsigned minor = (fwver >> (8 * 2)) & 0xFF;
				unsigned patch = (fwver >> (8 * 1)) & 0xFF;
				unsigned type = (fwver >> (8 * 0)) & 0xFF;
				printf("FW version: %s (%u.%u.%u %u), %u\n", px4_git_tag, major, minor, patch,
				       type, fwver);
				/* middleware is currently the same thing as firmware, so not printing yet */
				printf("OS version: %s (%u)\n", os_git_tag, version_tag_to_number(os_git_tag));
				ret = 0;

			}

			if (show_all || !strncmp(argv[1], sz_ver_bdate_str, sizeof(sz_ver_bdate_str))) {
				printf("Build datetime: %s %s\n", __DATE__, __TIME__);
				ret = 0;

			}

			if (show_all || !strncmp(argv[1], sz_ver_gcc_str, sizeof(sz_ver_gcc_str))) {
				printf("Toolchain: %s\n", __VERSION__);
				ret = 0;

			}

			if (show_all || !strncmp(argv[1], mcu_ver_str, sizeof(mcu_ver_str))) {

				char rev;
				char *revstr;

				int chip_version = mcu_version(&rev, &revstr);

				if (chip_version < 0) {
					printf("UNKNOWN MCU\n");

				} else {
					printf("MCU: %s, rev. %c\n", revstr, rev);

					if (chip_version < MCU_REV_STM32F4_REV_3) {
						printf("\nWARNING   WARNING   WARNING!\n"
						       "Revision %c has a silicon errata\n"
						       "This device can only utilize a maximum of 1MB flash safely!\n"
						       "https://pixhawk.org/help/errata\n\n", rev);
					}
				}

				ret = 0;
			}

			if (show_all || !strncmp(argv[1], mcu_uid_str, sizeof(mcu_uid_str))) {
				uint32_t uid[3];

				mcu_unique_id(uid);

				printf("UID: %X:%X:%X \n", uid[0], uid[1], uid[2]);

				ret = 0;
			}


			if (ret == 1) {
				warn("unknown command.\n");
				return 1;
//.........这里部分代码省略.........

void
Syslink::handle_raw_other(syslink_message_t *sys)
{
	// This function doesn't actually do anything
	// It is just here to return null responses to most standard messages

	crtp_message_t *c = (crtp_message_t *) &sys->length;

	if (c->port == CRTP_PORT_LOG) {

		PX4_INFO("Log: %d %d", c->channel, c->data[0]);

		if (c->channel == 0) { // Table of Contents Access

			uint8_t cmd = c->data[0];

			if (cmd == 0) { // GET_ITEM
				//int id = c->data[1];
				memset(&c->data[2], 0, 3);
				c->data[2] = 1; // type
				c->size = 1 + 5;
				send_message(sys);

			} else if (cmd == 1) { // GET_INFO
				memset(&c->data[1], 0, 7);
				c->size = 1 + 8;
				send_message(sys);
			}

		} else if (c->channel == 1) { // Log control

			uint8_t cmd = c->data[0];

			PX4_INFO("Responding to cmd: %d", cmd);
			c->data[2] = 0; // Success
			c->size = 3 + 1;

			// resend message
			send_message(sys);

		} else if (c->channel == 2) { // Log data

		}
	} else if (c->port == CRTP_PORT_MEM) {
		if (c->channel == 0) { // Info
			int cmd = c->data[0];

			if (cmd == 1) { // GET_NBR_OF_MEMS
				c->data[1] = 0;
				c->size = 2 + 1;

				// resend message
				send_message(sys);
			}
		}

	} else if (c->port == CRTP_PORT_PARAM) {
		if (c->channel == 0) { // TOC Access
			//	uint8_t msgId = c->data[0];

			c->data[1] = 0; // Last parameter (id = 0)
			memset(&c->data[2], 0, 10);
			c->size = 1 + 8;
			send_message(sys);
		}

		else if (c->channel == 1) { // Param read
			// 0 is ok
			c->data[1] = 0; // value
			c->size = 1 + 2;
			send_message(sys);
		}

	} else {
		PX4_INFO("Got raw: %d", c->port);
	}
}

void BlockLocalPositionEstimator::gpsInit()
{
	// check for good gps signal
	uint8_t nSat = _sub_gps.get().satellites_used;
	float eph = _sub_gps.get().eph;
	float epv = _sub_gps.get().epv;
	uint8_t fix_type = _sub_gps.get().fix_type;

	if (
		nSat < 6 ||
		eph > _gps_eph_max.get() ||
		epv > _gps_epv_max.get() ||
		fix_type < 3
	) {
		_gpsStats.reset();
		return;
	}

	// measure
	Vector<double, n_y_gps> y;

	if (gpsMeasure(y) != OK) {
		_gpsStats.reset();
		return;
	}

	// if finished
	if (_gpsStats.getCount() > REQ_GPS_INIT_COUNT) {
		// get mean gps values
		double gpsLat = _gpsStats.getMean()(0);
		double gpsLon = _gpsStats.getMean()(1);
		float gpsAlt = _gpsStats.getMean()(2);

		_sensorTimeout &= ~SENSOR_GPS;
		_sensorFault &= ~SENSOR_GPS;
		_gpsStats.reset();

		if (!_receivedGps) {
			// this is the first time we have received gps
			_receivedGps = true;

			// note we subtract X_z which is in down directon so it is
			// an addition
			_gpsAltOrigin = gpsAlt + _x(X_z);

			// find lat, lon of current origin by subtracting x and y
			// if not using vision position since vision will
			// have it's own origin, not necessarily where vehicle starts
			if (!(_fusion.get() & FUSE_VIS_POS)) {
				double gpsLatOrigin = 0;
				double gpsLonOrigin = 0;
				// reproject at current coordinates
				map_projection_init(&_map_ref, gpsLat, gpsLon);
				// find origin
				map_projection_reproject(&_map_ref, -_x(X_x), -_x(X_y), &gpsLatOrigin, &gpsLonOrigin);
				// reinit origin
				map_projection_init(&_map_ref, gpsLatOrigin, gpsLonOrigin);

				// always override alt origin on first GPS to fix
				// possible baro offset in global altitude at init
				_altOrigin = _gpsAltOrigin;
				_altOriginInitialized = true;
			}

			PX4_INFO("[lpe] gps init "
				 "lat %6.2f lon %6.2f alt %5.1f m",
				 gpsLat,
				 gpsLon,
				 double(gpsAlt));
		}
	}
}

/**
 * Perform some basic functional tests on the driver;
 * make sure we can collect data from the sensor in polled
 * and automatic modes.
 */
void
test()
{
	PX4_INFO("PASS");
}

void
Syslink::handle_message(syslink_message_t *msg)
{
	hrt_abstime t = hrt_absolute_time();

	if (t - _lasttime > 1000000) {
		pktrate = _count;
		rxrate = _count_in;
		txrate = _count_out;
		nullrate = _null_count;

		_lasttime = t;
		_count = 0;
		_null_count = 0;
		_count_in = 0;
		_count_out = 0;
	}

	_count++;

	if (msg->type == SYSLINK_PM_ONOFF_SWITCHOFF) {
		// When the power button is hit
	} else if (msg->type == SYSLINK_PM_BATTERY_STATE) {

		if (msg->length != 9) {
			return;
		}

		uint8_t flags = msg->data[0];
		int charging = flags & 1;
		int powered = flags & 2;

		float vbat; //, iset;
		memcpy(&vbat, &msg->data[1], sizeof(float));
		//memcpy(&iset, &msg->data[5], sizeof(float));

		_battery.updateBatteryStatus(t, vbat, -1, true, true, 0, 0, false, &_battery_status);


		// Update battery charge state
		if (charging) {
			_bstate = BAT_CHARGING;
		}

		/* With the usb plugged in and battery disconnected, it appears to be charged. The voltage check ensures that a battery is connected  */
		else if (powered && !charging && _battery_status.voltage_filtered_v > 3.7f) {
			_bstate = BAT_CHARGED;

		} else {
			_bstate = BAT_DISCHARGING;
		}


		// announce the battery status if needed, just publish else
		if (_battery_pub != nullptr) {
			orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);

		} else {
			_battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status);
		}

	} else if (msg->type == SYSLINK_RADIO_RSSI) {
		uint8_t rssi = msg->data[0]; // Between 40 and 100 meaning -40 dBm to -100 dBm
		_rssi = 140 - rssi * 100 / (100 - 40);

	} else if (msg->type == SYSLINK_RADIO_RAW) {
		handle_raw(msg);
		_lastrxtime = t;

	} else if ((msg->type & SYSLINK_GROUP) == SYSLINK_RADIO) {
		handle_radio(msg);

	} else if ((msg->type & SYSLINK_GROUP) == SYSLINK_OW) {
		memcpy(&_memory->msgbuf, msg, sizeof(syslink_message_t));
		px4_sem_post(&memory_sem);

	} else {
		PX4_INFO("GOT %d", msg->type);
	}

	//Send queued messages
	if (!_queue.empty()) {
		_queue.get(msg, sizeof(syslink_message_t));
		send_message(msg);
	}


	float p = (t % 500000) / 500000.0f;

	/* Use LED_GREEN for charging indicator */
	if (_bstate == BAT_CHARGED) {
		led_on(LED_GREEN);

	} else if (_bstate == BAT_CHARGING && p < 0.25f) {
		led_on(LED_GREEN);

	} else {
		led_off(LED_GREEN);
	}

//.........这里部分代码省略.........

int
tfmini_main(int argc, char *argv[])
{
	// check for optional arguments
	int ch;
	uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
	const char *device_path = "";
	int myoptind = 1;
	const char *myoptarg = nullptr;


	while ((ch = px4_getopt(argc, argv, "R:d:", &myoptind, &myoptarg)) != EOF) {
		switch (ch) {
		case 'R':
			rotation = (uint8_t)atoi(myoptarg);
			PX4_INFO("Setting distance sensor orientation to %d", (int)rotation);
			break;

		case 'd':
			device_path = myoptarg;
			PX4_INFO("Using device path '%s'", device_path);
			break;

		default:
			PX4_WARN("Unknown option!");
		}
	}

	/*
	 * Start/load the driver.
	 */
	if (!strcmp(argv[myoptind], "start")) {
		if (strcmp(device_path, "") != 0) {
			tfmini::start(device_path, rotation);

		} else {
			PX4_WARN("Please specify device path!");
			tfmini::usage();
			return PX4_ERROR;
		}
	}

	/*
	 * Stop the driver
	 */
	if (!strcmp(argv[myoptind], "stop")) {
		tfmini::stop();
	}

	/*
	 * Test the driver/device.
	 */
	if (!strcmp(argv[myoptind], "test")) {
		tfmini::test();
	}

	/*
	 * Reset the driver.
	 */
	if (!strcmp(argv[myoptind], "reset")) {
		tfmini::reset();
	}

	/*
	 * Print driver information.
	 */
	if (!strcmp(argv[myoptind], "info") || !strcmp(argv[1], "status")) {
		tfmini::info();
	}

	PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'");
	return PX4_ERROR;
}

static int usage()
{
	PX4_INFO("usage: camera_trigger {start|stop|status|test}\n");
	return 1;
}

void
Navigator::task_main()
{
	bool have_geofence_position_data = false;

	/* Try to load the geofence:
	 * if /fs/microsd/etc/geofence.txt load from this file */
	struct stat buffer;

	if (stat(GEOFENCE_FILENAME, &buffer) == 0) {
		PX4_INFO("Loading geofence from %s", GEOFENCE_FILENAME);
		_geofence.loadFromFile(GEOFENCE_FILENAME);
	}

	/* do subscriptions */
	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
	_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
	_gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
	_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
	_fw_pos_ctrl_status_sub = orb_subscribe(ORB_ID(fw_pos_ctrl_status));
	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
	_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
	_home_pos_sub = orb_subscribe(ORB_ID(home_position));
	_offboard_mission_sub = orb_subscribe(ORB_ID(mission));
	_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
	_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
	_traffic_sub = orb_subscribe(ORB_ID(transponder_report));

	/* copy all topics first time */
	vehicle_status_update();
	vehicle_land_detected_update();
	global_position_update();
	local_position_update();
	gps_position_update();
	sensor_combined_update();
	home_position_update(true);
	fw_pos_ctrl_status_update(true);
	params_update();

	/* wakeup source(s) */
	px4_pollfd_struct_t fds[1] = {};

	/* Setup of loop */
	fds[0].fd = _local_pos_sub;
	fds[0].events = POLLIN;

	/* rate-limit position subscription to 20 Hz / 50 ms */
	orb_set_interval(_local_pos_sub, 50);

	while (!_task_should_exit) {

		/* wait for up to 1000ms for data */
		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000);

		if (pret == 0) {
			/* Let the loop run anyway, don't do `continue` here. */

		} else if (pret < 0) {
			/* this is undesirable but not much we can do - might want to flag unhappy status */
			PX4_ERR("poll error %d, %d", pret, errno);
			usleep(10000);
			continue;

		} else {
			if (fds[0].revents & POLLIN) {
				/* success, local pos is available */
				local_position_update();
			}
		}

		perf_begin(_loop_perf);

		bool updated;

		/* gps updated */
		orb_check(_gps_pos_sub, &updated);

		if (updated) {
			gps_position_update();

			if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) {
				have_geofence_position_data = true;
			}
		}

		/* global position updated */
		orb_check(_global_pos_sub, &updated);

		if (updated) {
			global_position_update();

			if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
				have_geofence_position_data = true;
			}
		}

		/* sensors combined updated */
		orb_check(_sensor_combined_sub, &updated);

		if (updated) {
//.........这里部分代码省略.........

void usage()
{
	PX4_INFO("usage: uart_esc start -d /dev/tty-3");
	PX4_INFO("       uart_esc stop");
	PX4_INFO("       uart_esc status");
}

static int micrortps_start(int argc, char *argv[])
{
	if (0 > parse_options(argc, argv)) {
		printf("EXITING...\n");
		_rtps_task = -1;
		return -1;
	}

	switch (_options.transport) {
	case options::eTransports::UART: {
			transport_node = new UART_node(_options.device, _options.baudrate, _options.poll_ms);
			printf("\nUART transport: device: %s; baudrate: %d; sleep: %dms; poll: %dms\n\n",
			       _options.device, _options.baudrate, _options.sleep_ms, _options.poll_ms);
		}
		break;

	case options::eTransports::UDP: {
			transport_node = new UDP_node(_options.recv_port, _options.send_port);
			printf("\nUDP transport: recv port: %u; send port: %u; sleep: %dms\n\n",
			       _options.recv_port, _options.send_port, _options.sleep_ms);
		}
		break;

	default:
		_rtps_task = -1;
		printf("EXITING...\n");
		return -1;
	}

	if (0 > transport_node->init()) {
		printf("EXITING...\n");
		_rtps_task = -1;
		return -1;
	}


	struct timespec begin;

	int total_read = 0, loop = 0;

	uint32_t received = 0;

	micrortps_start_topics(begin, total_read, received, loop);

	struct timespec end;

	px4_clock_gettime(CLOCK_REALTIME, &end);

	double elapsed_secs = double(end.tv_sec - begin.tv_sec) + double(end.tv_nsec - begin.tv_nsec) / double(1000000000);

	printf("RECEIVED: %lu messages in %d LOOPS, %d bytes in %.03f seconds - %.02fKB/s\n\n",
	       (unsigned long)received, loop, total_read, elapsed_secs, (double)total_read / (1000 * elapsed_secs));

	delete transport_node;

	transport_node = nullptr;

	PX4_INFO("exiting");

	fflush(stdout);

	_rtps_task = -1;

	return 0;
}

void InputTest::print_status()
{
	PX4_INFO("Input: Test");
}

//.........这里部分代码省略.........
						}
					}
				}

			}

			// alternate between S.port and D-type baud rates
			if (baudRate == SPORT) {
				PX4_DEBUG("setting baud rate to %d", 9600);
				set_uart_speed(uart, &uart_config, B9600);
				baudRate = DTYPE;

			} else {
				PX4_DEBUG("setting baud rate to %d", 57600);
				set_uart_speed(uart, &uart_config, B57600);
				baudRate = SPORT;

			}

			// wait a second
			usleep(1000000);
			// flush buffer
			read(uart, &sbuf[0], sizeof(sbuf));

		}
	}

	if (frsky_state == SPORT) {
		/* Subscribe to topics */
		if (!sPort_init()) {
			err(1, "could not allocate memory");
		}

		PX4_INFO("sending FrSky SmartPort telemetry");

		struct sensor_baro_s *sensor_baro = malloc(sizeof(struct sensor_baro_s));

		if (sensor_baro == NULL) {
			err(1, "could not allocate memory");
		}

		static float filtered_alt = NAN;
		int sensor_sub = orb_subscribe(ORB_ID(sensor_baro));

		/* send S.port telemetry */
		while (!thread_should_exit) {

			/* wait for poll frame starting with value 0x7E
			* note that only the bus master is supposed to put a 0x7E on the bus.
			* slaves use byte stuffing to send 0x7E and 0x7D.
			* we expect a poll frame every 12msec
			*/
			int status = poll(fds, sizeof(fds) / sizeof(fds[0]), 50);

			if (status < 1) { continue; }

			// read 1 byte
			int newBytes = read(uart, &sbuf[0], 1);

			if (newBytes < 1 || sbuf[0] != 0x7E) { continue; }

			/* wait for ID byte */
			status = poll(fds, sizeof(fds) / sizeof(fds[0]), 50);

			if (status < 1) { continue; }

int
param_main(int argc, char *argv[])
{
	if (argc >= 2) {
		if (!strcmp(argv[1], "save")) {
			if (argc >= 3) {
				return do_save(argv[2]);

			} else {
				int ret = do_save_default();

				if (ret) {
					PX4_ERR("Param save failed (%i)", ret);
					return 1;

				} else {
					return 0;
				}
			}
		}

		if (!strcmp(argv[1], "load")) {
			if (argc >= 3) {
				return do_load(argv[2]);

			} else {
				return do_load(param_get_default_file());
			}
		}

		if (!strcmp(argv[1], "import")) {
			if (argc >= 3) {
				return do_import(argv[2]);

			} else {
				return do_import(param_get_default_file());
			}
		}

		if (!strcmp(argv[1], "select")) {
			if (argc >= 3) {
				param_set_default_file(argv[2]);

			} else {
				param_set_default_file(NULL);
			}

			PX4_INFO("selected parameter default file %s", param_get_default_file());
			return 0;
		}

		if (!strcmp(argv[1], "show")) {
			if (argc >= 3) {
				// optional argument -c to show only non-default params
				if (!strcmp(argv[2], "-c")) {
					if (argc >= 4) {
						return do_show(argv[3], true);

					} else {
						return do_show(NULL, true);
					}

				} else {
					return do_show(argv[2], false);
				}

			} else {
				return do_show(NULL, false);
			}
		}

		if (!strcmp(argv[1], "set")) {
			if (argc >= 5) {

				/* if the fail switch is provided, fails the command if not found */
				bool fail = !strcmp(argv[4], "fail");

				return do_set(argv[2], argv[3], fail);

			} else if (argc >= 4) {
				return do_set(argv[2], argv[3], false);

			} else {
				PX4_ERR("not enough arguments.\nTry 'param set PARAM_NAME 3 [fail]'");
				return 1;
			}
		}

		if (!strcmp(argv[1], "compare")) {
			if (argc >= 4) {
				return do_compare(argv[2], &argv[3], argc - 3, COMPARE_OPERATOR_EQUAL);

			} else {
				PX4_ERR("not enough arguments.\nTry 'param compare PARAM_NAME 3'");
				return 1;
			}
		}

		if (!strcmp(argv[1], "greater")) {
			if (argc >= 4) {
//.........这里部分代码省略.........

//.........这里部分代码省略.........
	}

	/* measurements completed successfully, rotate calibration values */
	param_t board_rotation_h = param_find("SENS_BOARD_ROT");
	int32_t board_rotation_int;
	param_get(board_rotation_h, &(board_rotation_int));
	enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
	math::Matrix<3, 3> board_rotation;
	get_rot_matrix(board_rotation_id, &board_rotation);
	math::Matrix<3, 3> board_rotation_t = board_rotation.transposed();

	bool tc_locked[3] = {false}; // true when the thermal parameter instance has already been adjusted by the calibrator

	for (unsigned uorb_index = 0; uorb_index < active_sensors; uorb_index++) {

		/* handle individual sensors, one by one */
		math::Vector<3> accel_offs_vec(accel_offs[uorb_index]);
		math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec;
		math::Matrix<3, 3> accel_T_mat(accel_T[uorb_index]);
		math::Matrix<3, 3> accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation;

		accel_scale.x_offset = accel_offs_rotated(0);
		accel_scale.x_scale = accel_T_rotated(0, 0);
		accel_scale.y_offset = accel_offs_rotated(1);
		accel_scale.y_scale = accel_T_rotated(1, 1);
		accel_scale.z_offset = accel_offs_rotated(2);
		accel_scale.z_scale = accel_T_rotated(2, 2);

		bool failed = false;

		failed = failed || (PX4_OK != param_set_no_notification(param_find("CAL_ACC_PRIME"), &(device_id_primary)));


		PX4_INFO("found offset %d: x: %.6f, y: %.6f, z: %.6f", uorb_index,
				(double)accel_scale.x_offset,
				(double)accel_scale.y_offset,
				(double)accel_scale.z_offset);
		PX4_INFO("found scale %d: x: %.6f, y: %.6f, z: %.6f", uorb_index,
				(double)accel_scale.x_scale,
				(double)accel_scale.y_scale,
				(double)accel_scale.z_scale);

		/* check if thermal compensation is enabled */
		int32_t tc_enabled_int;
		param_get(param_find("TC_A_ENABLE"), &(tc_enabled_int));
		if (tc_enabled_int == 1) {
			/* Get struct containing sensor thermal compensation data */
			struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */
			memset(&sensor_correction, 0, sizeof(sensor_correction));
			int sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction));
			orb_copy(ORB_ID(sensor_correction), sensor_correction_sub, &sensor_correction);
			orb_unsubscribe(sensor_correction_sub);

			/* don't allow a parameter instance to be calibrated more than once by another uORB instance */
			if (!tc_locked[sensor_correction.accel_mapping[uorb_index]]) {
				tc_locked[sensor_correction.accel_mapping[uorb_index]] = true;

				/* update the _X0_ terms to include the additional offset */
				int32_t handle;
				float val;
				for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
					val = 0.0f;
					(void)sprintf(str, "TC_A%u_X0_%u", sensor_correction.accel_mapping[uorb_index], axis_index);
					handle = param_find(str);
					param_get(handle, &val);
					if (axis_index == 0) {