void
FixedwingAttitudeControl::task_main()
{
	/*
	 * do subscriptions
	 */
	_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
	_accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
	_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
	_params_sub = orb_subscribe(ORB_ID(parameter_update));
	_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));

	/* rate limit vehicle status updates to 5Hz */
	orb_set_interval(_vcontrol_mode_sub, 200);
	/* do not limit the attitude updates in order to minimize latency.
	 * actuator outputs are still limited by the individual drivers
	 * properly to not saturate IO or physical limitations */

	parameters_update();

	/* get an initial update for all sensor and status data */
	vehicle_airspeed_poll();
	vehicle_setpoint_poll();
	vehicle_accel_poll();
	vehicle_control_mode_poll();
	vehicle_manual_poll();
	vehicle_status_poll();

	/* wakeup source(s) */
	struct pollfd fds[2];

	/* Setup of loop */
	fds[0].fd = _params_sub;
	fds[0].events = POLLIN;
	fds[1].fd = _att_sub;
	fds[1].events = POLLIN;

	_task_running = true;

	while (!_task_should_exit) {

		static int loop_counter = 0;

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

		/* timed out - periodic check for _task_should_exit, etc. */
		if (pret == 0)
			continue;

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

		perf_begin(_loop_perf);

		/* only update parameters if they changed */
		if (fds[0].revents & POLLIN) {
			/* read from param to clear updated flag */
			struct parameter_update_s update;
			orb_copy(ORB_ID(parameter_update), _params_sub, &update);

			/* update parameters from storage */
			parameters_update();
		}

		/* only run controller if attitude changed */
		if (fds[1].revents & POLLIN) {


			static uint64_t last_run = 0;
			float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
			last_run = hrt_absolute_time();

			/* guard against too large deltaT's */
			if (deltaT > 1.0f)
				deltaT = 0.01f;

			/* load local copies */
			orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);

			if (_vehicle_status.is_vtol && _parameters.vtol_type == 0) {
				/* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode
				 *
				 * Since the VTOL airframe is initialized as a multicopter we need to
				 * modify the estimated attitude for the fixed wing operation.
				 * Since the neutral position of the vehicle in fixed wing mode is -90 degrees rotated around
				 * the pitch axis compared to the neutral position of the vehicle in multicopter mode
				 * we need to swap the roll and the yaw axis (1st and 3rd column) in the rotation matrix.
				 * Additionally, in order to get the correct sign of the pitch, we need to multiply
				 * the new x axis of the rotation matrix with -1
				 *
				 * original:			modified:
				 *
//.........这里部分代码省略.........

//.........这里部分代码省略.........
	memset(&local_pos, 0, sizeof(local_pos));
	struct optical_flow_s flow;
	memset(&flow, 0, sizeof(flow));
	struct vision_position_estimate_s vision;
	memset(&vision, 0, sizeof(vision));
	struct att_pos_mocap_s mocap;
	memset(&mocap, 0, sizeof(mocap));
	struct vehicle_global_position_s global_pos;
	memset(&global_pos, 0, sizeof(global_pos));

	/* subscribe */
	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
	int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
	int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
	int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
	int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
	int vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate));
	int att_pos_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
	int home_position_sub = orb_subscribe(ORB_ID(home_position));

	/* advertise */
	orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);
	orb_advert_t vehicle_global_position_pub = NULL;

	struct position_estimator_inav_params params;
	struct position_estimator_inav_param_handles pos_inav_param_handles;
	/* initialize parameter handles */
	inav_parameters_init(&pos_inav_param_handles);

	/* first parameters read at start up */
	struct parameter_update_s param_update;
	orb_copy(ORB_ID(parameter_update), parameter_update_sub, &param_update); /* read from param topic to clear updated flag */
	/* first parameters update */
	inav_parameters_update(&pos_inav_param_handles, &params);

	px4_pollfd_struct_t fds_init[1] = {
		{ .fd = sensor_combined_sub, .events = POLLIN },
	};

	/* wait for initial baro value */
	bool wait_baro = true;

	thread_running = true;

	while (wait_baro && !thread_should_exit) {
		int ret = px4_poll(fds_init, 1, 1000);

		if (ret < 0) {
			/* poll error */
			mavlink_log_info(mavlink_fd, "[inav] poll error on init");

		} else if (ret > 0) {
			if (fds_init[0].revents & POLLIN) {
				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);

				if (wait_baro && sensor.baro_timestamp[0] != baro_timestamp) {
					baro_timestamp = sensor.baro_timestamp[0];

					/* mean calculation over several measurements */
					if (baro_init_cnt < baro_init_num) {
						if (PX4_ISFINITE(sensor.baro_alt_meter[0])) {
							baro_offset += sensor.baro_alt_meter[0];
							baro_init_cnt++;
						}

void
Navigator::gps_position_update()
{
	orb_copy(ORB_ID(vehicle_gps_position), _gps_pos_sub, &_gps_pos);
}

void task_main(int argc, char *argv[])
{
	_is_running = true;

	if (uart_initialize(_device) < 0) {
		PX4_ERR("Failed to initialize UART.");
		return;
	}

	// Subscribe for orb topics
	_controls_sub = orb_subscribe(ORB_ID(actuator_controls_0));
	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));

	// Start disarmed
	_armed.armed = false;
	_armed.prearmed = false;

	// Set up poll topic
	px4_pollfd_struct_t fds[1];
	fds[0].fd     = _controls_sub;
	fds[0].events = POLLIN;
	/* Don't limit poll intervall for now, 250 Hz should be fine. */
	//orb_set_interval(_controls_sub, 10);

	// Set up mixer
	if (initialize_mixer(MIXER_FILENAME) < 0) {
		PX4_ERR("Mixer initialization failed.");
		return;
	}

	pwm_limit_init(&_pwm_limit);

	// TODO XXX: this is needed otherwise we crash in the callback context.
	_rc_pub = orb_advertise(ORB_ID(input_rc), &_rc);

	// Main loop
	while (!_task_should_exit) {

		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 10);

		/* Timed out, do a periodic check for _task_should_exit. */
		if (pret == 0) {
			continue;
		}

		/* This is undesirable but not much we can do. */
		if (pret < 0) {
			PX4_WARN("poll error %d, %d", pret, errno);
			/* sleep a bit before next try */
			usleep(100000);
			continue;
		}

		if (fds[0].revents & POLLIN) {
			orb_copy(ORB_ID(actuator_controls_0), _controls_sub, &_controls);

			_outputs.timestamp = _controls.timestamp;

			/* do mixing */
			_outputs.noutputs = _mixer->mix(_outputs.output, 0 /* not used */, NULL);

			/* disable unused ports by setting their output to NaN */
			for (size_t i = _outputs.noutputs; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) {
				_outputs.output[i] = NAN;
			}

			const uint16_t reverse_mask = 0;
			uint16_t disarmed_pwm[4];
			uint16_t min_pwm[4];
			uint16_t max_pwm[4];

			for (unsigned int i = 0; i < 4; i++) {
				disarmed_pwm[i] = _pwm_disarmed;
				min_pwm[i] = _pwm_min;
				max_pwm[i] = _pwm_max;
			}

			uint16_t pwm[4];

			// TODO FIXME: pre-armed seems broken
			pwm_limit_calc(_armed.armed, false/*_armed.prearmed*/, _outputs.noutputs, reverse_mask,
				       disarmed_pwm, min_pwm, max_pwm, _outputs.output, pwm, &_pwm_limit);


			send_outputs_mavlink(pwm, 4);

			if (_outputs_pub != nullptr) {
				orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs);

			} else {
				_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs);
			}
		}

		bool updated;
		orb_check(_armed_sub, &updated);

		if (updated) {
			orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
		}
//.........这里部分代码省略.........

//.........这里部分代码省略.........
	{
		fprintf (stderr, "Comunicator thread failed to advertise the vehicle_attitude_setpoint topic\n");
		exit (-1);
	}
	orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);



	/* abort on a nonzero return value from the parameter init */
	if (multirotor_position_control_params_init() != 0) {
		/* parameter setup went wrong, abort */
		fprintf (stderr, "Multirotor position controller aborting on startup due to an error\n");
		exit(-1);
	}

	for (i = 0; i < 2; i++) {
		pid_init(&(xy_pos_pids[i]), multirotor_position_control_parameters.xy_p, 0.0f,
				multirotor_position_control_parameters.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f);
		pid_init(&(xy_vel_pids[i]), multirotor_position_control_parameters.xy_vel_p, multirotor_position_control_parameters.xy_vel_i,
				multirotor_position_control_parameters.xy_vel_d, 1.0f, multirotor_position_control_parameters.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
	}

	pid_init(&z_pos_pid, multirotor_position_control_parameters.z_p, 0.0f,
			multirotor_position_control_parameters.z_d, 1.0f, multirotor_position_control_parameters.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f);
	thrust_pid_init(&z_vel_pid, multirotor_position_control_parameters.z_vel_p, multirotor_position_control_parameters.z_vel_i,
			multirotor_position_control_parameters.z_vel_d, -multirotor_position_control_parameters.thr_max, -multirotor_position_control_parameters.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);


	while (!_shutdown_all_systems) {

		updated = orb_check (ORB_ID(parameter_update), param_sub);
		if (updated) {
			/* clear updated flag */
			orb_copy(ORB_ID(parameter_update), param_sub, &ps);

			/* update multirotor_position_control_parameters */
			multirotor_position_control_params_update();

			for (i = 0; i < 2; i++) {
				pid_set_parameters(&(xy_pos_pids[i]), multirotor_position_control_parameters.xy_p,
						0.0f, multirotor_position_control_parameters.xy_d, 1.0f, 0.0f);

				if (multirotor_position_control_parameters.xy_vel_i > 0.0f) {
					i_limit = multirotor_position_control_parameters.tilt_max / multirotor_position_control_parameters.xy_vel_i / 2.0f;

				} else {
					i_limit = 0.0f;	// not used
				}

				pid_set_parameters(&(xy_vel_pids[i]), multirotor_position_control_parameters.xy_vel_p,
						multirotor_position_control_parameters.xy_vel_i, multirotor_position_control_parameters.xy_vel_d, i_limit, multirotor_position_control_parameters.tilt_max);
			}

			pid_set_parameters(&z_pos_pid, multirotor_position_control_parameters.z_p, 0.0f,
					multirotor_position_control_parameters.z_d, 1.0f, multirotor_position_control_parameters.z_vel_max);
			thrust_pid_set_parameters(&z_vel_pid, multirotor_position_control_parameters.z_vel_p, multirotor_position_control_parameters.z_vel_i,
					multirotor_position_control_parameters.z_vel_d, -multirotor_position_control_parameters.thr_max, -multirotor_position_control_parameters.thr_min);
		}

		updated = orb_check (ORB_ID(vehicle_control_flags), control_flags_sub);
		if (updated) {
			orb_copy(ORB_ID(vehicle_control_flags), control_flags_sub, &control_flags);
		}

		updated = orb_check (ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub);
		if (updated) {

void
MulticopterAttitudeControl::task_main()
{
	warnx("started");
	fflush(stdout);

	/*
	 * do subscriptions
	 */
	_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
	_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
	_v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
	_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
	_params_sub = orb_subscribe(ORB_ID(parameter_update));
	_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));

	/* initialize parameters cache */
	parameters_update();

	/* wakeup source: vehicle attitude */
	struct pollfd fds[1];

	fds[0].fd = _v_att_sub;
	fds[0].events = POLLIN;

	while (!_task_should_exit) {

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

		/* timed out - periodic check for _task_should_exit */
		if (pret == 0)
			continue;

		/* this is undesirable but not much we can do - might want to flag unhappy status */
		if (pret < 0) {
			warn("poll error %d, %d", pret, errno);
			/* sleep a bit before next try */
			usleep(100000);
			continue;
		}

		perf_begin(_loop_perf);

		/* run controller on attitude changes */
		if (fds[0].revents & POLLIN) {
			static uint64_t last_run = 0;
			float dt = (hrt_absolute_time() - last_run) / 1000000.0f;
			last_run = hrt_absolute_time();

			/* guard against too small (< 2ms) and too large (> 20ms) dt's */
			if (dt < 0.002f) {
				dt = 0.002f;

			} else if (dt > 0.02f) {
				dt = 0.02f;
			}

			/* copy attitude topic */
			orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att);

			/* check for updates in other topics */
			parameter_update_poll();
			vehicle_control_mode_poll();
			arming_status_poll();
			vehicle_manual_poll();

			if (_v_control_mode.flag_control_attitude_enabled) {
				control_attitude(dt);

				/* publish attitude rates setpoint */
				_v_rates_sp.roll = _rates_sp(0);
				_v_rates_sp.pitch = _rates_sp(1);
				_v_rates_sp.yaw = _rates_sp(2);
				_v_rates_sp.thrust = _thrust_sp;
				_v_rates_sp.timestamp = hrt_absolute_time();

				if (_v_rates_sp_pub > 0) {
					orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);

				} else {
					_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
				}

			} else {
				/* attitude controller disabled, poll rates setpoint topic */
				if (_v_control_mode.flag_control_manual_enabled) {
					/* manual rates control - ACRO mode */
					_rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max);
					_thrust_sp = (_manual_control_sp.z + 1) / 2;

					/* reset yaw setpoint after ACRO */
					_reset_yaw_sp = true;

					/* publish attitude rates setpoint */
					_v_rates_sp.roll = _rates_sp(0);
					_v_rates_sp.pitch = _rates_sp(1);
					_v_rates_sp.yaw = _rates_sp(2);
					_v_rates_sp.thrust = _thrust_sp;
//.........这里部分代码省略.........

//.........这里部分代码省略.........
	/* initialize multiplexing, deactivate all outputs - must happen after UART open to claim GPIOs on PX4FMU */
	gpios = ar_multiplexing_init();

	if (ardrone_write < 0) {
		fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n");
		thread_running = false;
		exit(ERROR);
	}

	/* initialize motors */
	if (OK != ar_init_motors(ardrone_write, gpios)) {
		close(ardrone_write);
		fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n");
		thread_running = false;
		exit(ERROR);
	}

	while (!thread_should_exit) {

		if (motor_test_mode) {
			/* set motors to idle speed */
			if (test_motor > 0 && test_motor < 5) {
				int motors[4] = {0, 0, 0, 0};
				motors[test_motor - 1] = 10;
				ardrone_write_motor_commands(ardrone_write, motors[0], motors[1], motors[2], motors[3]);
			} else {
				ardrone_write_motor_commands(ardrone_write, 10, 10, 10, 10);
			}

		} else {
			/* MAIN OPERATION MODE */

			/* get a local copy of the vehicle state */
			orb_copy(ORB_ID(vehicle_status), state_sub, &state);
			/* get a local copy of the actuator controls */
			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
			orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
			
			/* for now only spin if armed and immediately shut down
			 * if in failsafe
			 */
			if (armed.armed && !armed.lockdown) {
				ardrone_mixing_and_output(ardrone_write, &actuator_controls);

			} else {
				/* Silently lock down motor speeds to zero */
				ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0);
			}
		}

		if (counter % 24 == 0) {
			if (led_counter == 0) ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0);

			if (led_counter == 1) ar_set_leds(ardrone_write, 1, 1, 0, 0, 0, 0, 0 , 0);

			if (led_counter == 2) ar_set_leds(ardrone_write, 1, 0, 0, 0, 0, 0, 0 , 0);

			if (led_counter == 3) ar_set_leds(ardrone_write, 0, 0, 0, 1, 0, 0, 0 , 0);

			if (led_counter == 4) ar_set_leds(ardrone_write, 0, 0, 1, 1, 0, 0, 0 , 0);

			if (led_counter == 5) ar_set_leds(ardrone_write, 0, 0, 1, 0, 0, 0, 0 , 0);

			if (led_counter == 6) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 1, 0 , 0);

			if (led_counter == 7) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 1, 0 , 0);

int uORBTest::UnitTest::test_multi2()
{

	test_note("Testing multi-topic 2 test (queue simulation)");
	//test: first subscribe, then advertise

	_thread_should_exit = false;
	const int num_instances = 3;
	int orb_data_fd[num_instances];
	int orb_data_next = 0;

	for (int i = 0; i < num_instances; ++i) {
//		PX4_WARN("subscribe %i, t=%" PRIu64, i, hrt_absolute_time());
		orb_data_fd[i] = orb_subscribe_multi(ORB_ID(orb_test_medium_multi), i);
	}

	char *const args[1] = { nullptr };
	int pubsub_task = px4_task_spawn_cmd("uorb_test_multi",
					     SCHED_DEFAULT,
					     SCHED_PRIORITY_MAX - 5,
					     2000,
					     (px4_main_t)&uORBTest::UnitTest::pub_test_multi2_entry,
					     args);

	if (pubsub_task < 0) {
		return test_fail("failed launching task");
	}

	hrt_abstime last_time = 0;

	while (!_thread_should_exit) {

		bool updated = false;
		int orb_data_cur_fd = orb_data_fd[orb_data_next];
		orb_check(orb_data_cur_fd, &updated);

		if (updated) {
			struct orb_test_medium msg;
			orb_copy(ORB_ID(orb_test_medium_multi), orb_data_cur_fd, &msg);

// Relax timing requirement for Darwin CI system
#ifdef __PX4_DARWIN
			usleep(10000);
#else
			usleep(1000);
#endif

			if (last_time >= msg.time && last_time != 0) {
				return test_fail("Timestamp not increasing! (%" PRIu64 " >= %" PRIu64 ")", last_time, msg.time);
			}

			last_time = msg.time;

//			PX4_WARN("      got message (val=%i, idx=%i, t=%" PRIu64 ")", msg.val, orb_data_next, msg.time);
			orb_data_next = (orb_data_next + 1) % num_instances;
		}
	}

	for (int i = 0; i < num_instances; ++i) {
		orb_unsubscribe(orb_data_fd[i]);
	}

	return test_note("PASS multi-topic 2 test (queue simulation)");
}

static int
flow_position_control_thread_main(int argc, char *argv[])
{
	/* welcome user */
	thread_running = true;
	printf("[flow position control] starting\n");

	uint32_t counter = 0;
	const float time_scale = powf(10.0f,-6.0f);

	/* structures */
	struct vehicle_status_s vstatus;
	struct vehicle_attitude_s att;
	struct manual_control_setpoint_s manual;
	struct filtered_bottom_flow_s filtered_flow;
	struct vehicle_local_position_s local_pos;

	struct vehicle_bodyframe_speed_setpoint_s speed_sp;

	/* subscribe to attitude, motor setpoints and system state */
	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
	int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
	int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
	int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow));
	int vehicle_local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));

	orb_advert_t speed_sp_pub;
	bool speed_setpoint_adverted = false;

	/* parameters init*/
	struct flow_position_control_params params;
	struct flow_position_control_param_handles param_handles;
	parameters_init(&param_handles);
	parameters_update(&param_handles, &params);

	/* init flow sum setpoint */
	float flow_sp_sumx = 0.0f;
	float flow_sp_sumy = 0.0f;

	/* init yaw setpoint */
	float yaw_sp = 0.0f;

	/* init height setpoint */
	float height_sp = params.height_min;

	/* height controller states */
	bool start_phase = true;
	bool landing_initialized = false;
	float landing_thrust_start = 0.0f;

	/* states */
	float integrated_h_error = 0.0f;
	float last_local_pos_z = 0.0f;
	bool update_flow_sp_sumx = false;
	bool update_flow_sp_sumy = false;
	uint64_t last_time = 0.0f;
	float dt = 0.0f; // s


	/* register the perf counter */
	perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "flow_position_control_runtime");
	perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "flow_position_control_interval");
	perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_position_control_err");

	static bool sensors_ready = false;

	while (!thread_should_exit)
	{
		/* wait for first attitude msg to be sure all data are available */
		if (sensors_ready)
		{
			/* polling */
			struct pollfd fds[2] = {
				{ .fd = filtered_bottom_flow_sub, .events = POLLIN }, // positions from estimator
				{ .fd = parameter_update_sub,   .events = POLLIN }

			};

			/* wait for a position update, check for exit condition every 500 ms */
			int ret = poll(fds, 2, 500);

			if (ret < 0)
			{
				/* poll error, count it in perf */
				perf_count(mc_err_perf);
			}
			else if (ret == 0)
			{
				/* no return value, ignore */
//				printf("[flow position control] no filtered flow updates\n");
			}
			else
			{
				/* parameter update available? */
				if (fds[1].revents & POLLIN)
				{
					/* read from param to clear updated flag */
					struct parameter_update_s update;
					orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
//.........这里部分代码省略.........

int do_gyro_calibration(int mavlink_fd)
{
	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
	mavlink_log_info(mavlink_fd, "don't move system");

	struct gyro_scale gyro_scale = {
		0.0f,
		1.0f,
		0.0f,
		1.0f,
		0.0f,
		1.0f,
	};

	int res = OK;

	/* reset all offsets to zero and all scales to one */
	int fd = open(GYRO_DEVICE_PATH, 0);
	res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale);
	close(fd);

	if (res != OK) {
		mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
	}

	if (res == OK) {
		/* determine gyro mean values */
		const unsigned calibration_count = 5000;
		unsigned calibration_counter = 0;
		unsigned poll_errcount = 0;

		/* subscribe to gyro sensor topic */
		int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro));
		struct gyro_report gyro_report;

		while (calibration_counter < calibration_count) {
			/* wait blocking for new data */
			struct pollfd fds[1];
			fds[0].fd = sub_sensor_gyro;
			fds[0].events = POLLIN;

			int poll_ret = poll(fds, 1, 1000);

			if (poll_ret > 0) {
				orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report);
				gyro_scale.x_offset += gyro_report.x;
				gyro_scale.y_offset += gyro_report.y;
				gyro_scale.z_offset += gyro_report.z;
				calibration_counter++;

				if (calibration_counter % (calibration_count / 20) == 0)
					mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count);

			} else {
				poll_errcount++;
			}

			if (poll_errcount > 1000) {
				mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
				res = ERROR;
				break;
			}
		}

		close(sub_sensor_gyro);

		gyro_scale.x_offset /= calibration_count;
		gyro_scale.y_offset /= calibration_count;
		gyro_scale.z_offset /= calibration_count;
	}

	if (res == OK) {
		/* check offsets */
		if (!isfinite(gyro_scale.x_offset) || !isfinite(gyro_scale.y_offset) || !isfinite(gyro_scale.z_offset)) {
			mavlink_log_critical(mavlink_fd, "ERROR: offset is NaN");
			res = ERROR;
		}
	}

	if (res == OK) {
		/* set offset parameters to new values */
		if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_scale.x_offset))
		    || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_scale.y_offset))
		    || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_scale.z_offset))) {
			mavlink_log_critical(mavlink_fd, "ERROR: failed to set offset params");
			res = ERROR;
		}
	}

#if 0
	/* beep on offset calibration end */
	mavlink_log_info(mavlink_fd, "gyro offset calibration done");
	tune_neutral();

	/* scale calibration */
	/* this was only a proof of concept and is currently not working. scaling will be set to 1.0 for now. */

	mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip.");
	warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip.");

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

static calibrate_return mag_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data)
{
	calibrate_return result = calibrate_return_ok;
	
	unsigned int calibration_counter_side;

	mag_worker_data_t* worker_data = (mag_worker_data_t*)(data);
	
	mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Rotate vehicle around the detected orientation");
	mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Continue rotation for %u seconds", worker_data->calibration_interval_perside_seconds);

	/*
	 * Detect if the system is rotating.
	 *
	 * We're detecting this as a general rotation on any axis, not necessary on the one we
	 * asked the user for. This is because we really just need two roughly orthogonal axes
	 * for a good result, so we're not constraining the user more than we have to.
	 */

	hrt_abstime detection_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds;
	hrt_abstime last_gyro = 0;
	float gyro_x_integral = 0.0f;
	float gyro_y_integral = 0.0f;
	float gyro_z_integral = 0.0f;

	const float gyro_int_thresh_rad = 0.5f;

	int sub_gyro = orb_subscribe(ORB_ID(sensor_gyro));

	while (fabsf(gyro_x_integral) < gyro_int_thresh_rad &&
		fabsf(gyro_y_integral) < gyro_int_thresh_rad &&
		fabsf(gyro_z_integral) < gyro_int_thresh_rad) {

		/* abort on request */
		if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) {
			result = calibrate_return_cancelled;
			px4_close(sub_gyro);
			return result;
		}

		/* abort with timeout */
		if (hrt_absolute_time() > detection_deadline) {
			result = calibrate_return_error;
			warnx("int: %8.4f, %8.4f, %8.4f", (double)gyro_x_integral, (double)gyro_y_integral, (double)gyro_z_integral);
			mavlink_and_console_log_critical(worker_data->mavlink_fd, "Failed: This calibration requires rotation.");
			break;
		}

		/* Wait clocking for new data on all gyro */
		px4_pollfd_struct_t fds[1];
		fds[0].fd = sub_gyro;
		fds[0].events = POLLIN;
		size_t fd_count = 1;

		int poll_ret = px4_poll(fds, fd_count, 1000);

		if (poll_ret > 0) {
			struct gyro_report gyro;
			orb_copy(ORB_ID(sensor_gyro), sub_gyro, &gyro);

			/* ensure we have a valid first timestamp */
			if (last_gyro > 0) {

				/* integrate */
				float delta_t = (gyro.timestamp - last_gyro) / 1e6f;
				gyro_x_integral += gyro.x * delta_t;
				gyro_y_integral += gyro.y * delta_t;
				gyro_z_integral += gyro.z * delta_t;
			}

			last_gyro = gyro.timestamp;
		}
	}

	px4_close(sub_gyro);
	
	uint64_t calibration_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds;
	unsigned poll_errcount = 0;
	
	calibration_counter_side = 0;
	
	while (hrt_absolute_time() < calibration_deadline &&
	       calibration_counter_side < worker_data->calibration_points_perside) {
		
		if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) {
			result = calibrate_return_cancelled;
			break;
		}
		
		// Wait clocking for new data on all mags
		px4_pollfd_struct_t fds[max_mags];
		size_t fd_count = 0;
		for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
			if (worker_data->sub_mag[cur_mag] >= 0) {
				fds[fd_count].fd = worker_data->sub_mag[cur_mag];
				fds[fd_count].events = POLLIN;
				fd_count++;
			}
		}
		int poll_ret = px4_poll(fds, fd_count, 1000);
//.........这里部分代码省略.........

void
Navigator::params_update()
{
	parameter_update_s param_update;
	orb_copy(ORB_ID(parameter_update), _param_update_sub, &param_update);
}

//.........这里部分代码省略.........
		/* vehicle control mode updated */
		orb_check(_control_mode_sub, &updated);
		if (updated) {
			vehicle_control_mode_update();
		}

		/* vehicle status updated */
		orb_check(_vstatus_sub, &updated);
		if (updated) {
			vehicle_status_update();
		}

		/* vehicle land detected updated */
		orb_check(_land_detected_sub, &updated);
		if (updated) {
			vehicle_land_detected_update();
		}

		/* navigation capabilities updated */
		orb_check(_capabilities_sub, &updated);
		if (updated) {
			navigation_capabilities_update();
		}

		/* home position updated */
		orb_check(_home_pos_sub, &updated);
		if (updated) {
			home_position_update();
		}

		orb_check(_vehicle_command_sub, &updated);
		if (updated) {
			vehicle_command_s cmd;
			orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);

			if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) {

				struct position_setpoint_triplet_s *rep = get_reposition_triplet();

				// store current position as previous position and goal as next
				rep->previous.yaw = NAN;
				rep->previous.lat = get_global_position()->lat;
				rep->previous.lon = get_global_position()->lon;
				rep->previous.alt = get_global_position()->alt;

				rep->current.loiter_radius = get_loiter_radius();
				rep->current.loiter_direction = 1;
				rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;

				// Go on and check which changes had been requested
				if (PX4_ISFINITE(cmd.param4)) {
					rep->current.yaw = cmd.param4;
				} else {
					rep->current.yaw = NAN;
				}

				if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) {
					rep->current.lat = cmd.param5 / (double)1e7;
					rep->current.lon = cmd.param6 / (double)1e7;
				} else {
					rep->current.lat = get_global_position()->lat;
					rep->current.lon = get_global_position()->lon;
				}

				if (PX4_ISFINITE(cmd.param7)) {
					rep->current.alt = cmd.param7;

int uORBTest::UnitTest::pubsublatency_main()
{
	/* poll on test topic and output latency */
	float latency_integral = 0.0f;

	/* wakeup source(s) */
	px4_pollfd_struct_t fds[3];

	int test_multi_sub = orb_subscribe_multi(ORB_ID(orb_test), 0);
	int test_multi_sub_medium = orb_subscribe_multi(ORB_ID(orb_test_medium), 0);
	int test_multi_sub_large = orb_subscribe_multi(ORB_ID(orb_test_large), 0);

	struct orb_test_large t;

	/* clear all ready flags */
	orb_copy(ORB_ID(orb_test), test_multi_sub, &t);
	orb_copy(ORB_ID(orb_test_medium), test_multi_sub_medium, &t);
	orb_copy(ORB_ID(orb_test_large), test_multi_sub_large, &t);

	fds[0].fd = test_multi_sub;
	fds[0].events = POLLIN;
	fds[1].fd = test_multi_sub_medium;
	fds[1].events = POLLIN;
	fds[2].fd = test_multi_sub_large;
	fds[2].events = POLLIN;

	const unsigned maxruns = 1000;
	unsigned timingsgroup = 0;
	int current_value = t.val;
	int num_missed = 0;

	// timings has to be on the heap to keep frame size below 2048 bytes
	unsigned *timings = new unsigned[maxruns];
	unsigned timing_min = 9999999, timing_max = 0;

	for (unsigned i = 0; i < maxruns; i++) {
		/* wait for up to 500ms for data */
		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500);

		if (fds[0].revents & POLLIN) {
			orb_copy(ORB_ID(orb_test), test_multi_sub, &t);
			timingsgroup = 0;

		} else if (fds[1].revents & POLLIN) {
			orb_copy(ORB_ID(orb_test_medium), test_multi_sub_medium, &t);
			timingsgroup = 1;

		} else if (fds[2].revents & POLLIN) {
			orb_copy(ORB_ID(orb_test_large), test_multi_sub_large, &t);
			timingsgroup = 2;
		}

		if (pret < 0) {
			PX4_ERR("poll error %d, %d", pret, errno);
			continue;
		}

		num_missed += t.val - current_value - 1;
		current_value = t.val;

		unsigned elt = (unsigned)hrt_elapsed_time(&t.time);
		latency_integral += elt;
		timings[i] = elt;

		if (elt > timing_max) {
			timing_max = elt;
		}

		if (elt < timing_min) {
			timing_min = elt;
		}
	}

	orb_unsubscribe(test_multi_sub);
	orb_unsubscribe(test_multi_sub_medium);
	orb_unsubscribe(test_multi_sub_large);

	if (pubsubtest_print) {
		char fname[32];
		sprintf(fname, PX4_STORAGEDIR"/uorb_timings%u.txt", timingsgroup);
		FILE *f = fopen(fname, "w");

		if (f == nullptr) {
			PX4_ERR("Error opening file!");
			delete[] timings;
			return PX4_ERROR;
		}

		for (unsigned i = 0; i < maxruns; i++) {
			fprintf(f, "%u\n", timings[i]);
		}

		fclose(f);
	}


	float std_dev = 0.f;
	float mean = latency_integral / maxruns;

	for (unsigned i = 0; i < maxruns; i++) {
//.........这里部分代码省略.........

void FlightTasks::_updateCommand()
{
	// lazy subscription to command topic
	if (_sub_vehicle_command < 0) {
		_sub_vehicle_command = orb_subscribe(ORB_ID(vehicle_command));
	}

	// check if there's any new command
	bool updated = false;
	orb_check(_sub_vehicle_command, &updated);

	if (!updated) {
		return;
	}

	// get command
	struct vehicle_command_s command;
	orb_copy(ORB_ID(vehicle_command), _sub_vehicle_command, &command);

	// check what command it is
	FlightTaskIndex desired_task = switchVehicleCommand(command.command);

	if (desired_task == FlightTaskIndex::None) {
		// ignore all unkown commands
		return;
	}

	// switch to the commanded task
	int switch_result = switchTask(desired_task);
	uint8_t cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;

	// if we are in/switched to the desired task
	if (switch_result >= 0) {
		cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;

		// if the task is running apply parameters to it and see if it rejects
		if (isAnyTaskActive() && !_current_task.task->applyCommandParameters(command)) {
			cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;

			// if we just switched and parameters are not accepted, go to failsafe
			if (switch_result == 1) {
				switchTask(FlightTaskIndex::ManualPosition);
				cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
			}
		}
	}

	// send back acknowledgment
	vehicle_command_ack_s command_ack = {};
	command_ack.command = command.command;
	command_ack.result = cmd_result;
	command_ack.result_param1 = switch_result;
	command_ack.target_system = command.source_system;
	command_ack.target_component = command.source_component;

	if (_pub_vehicle_command_ack == nullptr) {
		_pub_vehicle_command_ack = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
					   vehicle_command_ack_s::ORB_QUEUE_LENGTH);

	} else {
		orb_publish(ORB_ID(vehicle_command_ack), _pub_vehicle_command_ack, &command_ack);

	}
}

pthread_addr_t UavcanServers::run(pthread_addr_t)
{
	prctl(PR_SET_NAME, "uavcan fw srv", 0);

	Lock lock(_subnode_mutex);

	/*
	Copy any firmware bundled in the ROMFS to the appropriate location on the
	SD card, unless the user has copied other firmware for that device.
	*/
	unpackFwFromROMFS(UAVCAN_FIRMWARE_PATH, UAVCAN_ROMFS_FW_PATH);

	/* the subscribe call needs to happen in the same thread,
	 * so not in the constructor */
	int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
	int param_request_sub = orb_subscribe(ORB_ID(uavcan_parameter_request));
	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));

	/* Set up shared service clients */
	_param_getset_client.setCallback(GetSetCallback(this, &UavcanServers::cb_getset));
	_param_opcode_client.setCallback(ExecuteOpcodeCallback(this, &UavcanServers::cb_opcode));
	_param_restartnode_client.setCallback(RestartNodeCallback(this, &UavcanServers::cb_restart));
	_enumeration_client.setCallback(EnumerationBeginCallback(this, &UavcanServers::cb_enumeration_begin));
	_enumeration_indication_sub.start(EnumerationIndicationCallback(this, &UavcanServers::cb_enumeration_indication));
	_enumeration_getset_client.setCallback(GetSetCallback(this, &UavcanServers::cb_enumeration_getset));
	_enumeration_save_client.setCallback(ExecuteOpcodeCallback(this, &UavcanServers::cb_enumeration_save));

	_count_in_progress = _param_in_progress = _param_list_in_progress = _cmd_in_progress = _param_list_all_nodes = false;
	memset(_param_counts, 0, sizeof(_param_counts));

	_esc_enumeration_active = false;
	memset(_esc_enumeration_ids, 0, sizeof(_esc_enumeration_ids));
	_esc_enumeration_index = 0;

	while (!_subnode_thread_should_exit) {

		if (_check_fw == true) {
			_check_fw = false;
			_node_info_retriever.invalidateAll();
		}

		const int spin_res = _subnode.spin(uavcan::MonotonicDuration::fromMSec(10));
		if (spin_res < 0) {
			warnx("node spin error %i", spin_res);
		}

		// Check for parameter requests (get/set/list)
		bool param_request_ready;
		orb_check(param_request_sub, &param_request_ready);

		if (param_request_ready && !_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
			struct uavcan_parameter_request_s request;
			orb_copy(ORB_ID(uavcan_parameter_request), param_request_sub, &request);

			if (_param_counts[request.node_id]) {
				/*
				 * We know how many parameters are exposed by this node, so
				 * process the request.
				 */
				if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_READ) {
					uavcan::protocol::param::GetSet::Request req;
					if (request.param_index >= 0) {
						req.index = request.param_index;
					} else {
						req.name = (char*)request.param_id;
					}

					int call_res = _param_getset_client.call(request.node_id, req);
					if (call_res < 0) {
						warnx("UAVCAN command bridge: couldn't send GetSet: %d", call_res);
					} else {
						_param_in_progress = true;
						_param_index = request.param_index;
					}
				} else if (request.message_type == MAVLINK_MSG_ID_PARAM_SET) {
					uavcan::protocol::param::GetSet::Request req;
					if (request.param_index >= 0) {
						req.index = request.param_index;
					} else {
						req.name = (char*)request.param_id;
					}

					if (request.param_type == MAV_PARAM_TYPE_REAL32) {
						req.value.to<uavcan::protocol::param::Value::Tag::real_value>() = request.real_value;
					} else if (request.param_type == MAV_PARAM_TYPE_UINT8) {
						req.value.to<uavcan::protocol::param::Value::Tag::boolean_value>() = request.int_value;
					} else {
						req.value.to<uavcan::protocol::param::Value::Tag::integer_value>() = request.int_value;
					}

					// Set the dirty bit for this node
					set_node_params_dirty(request.node_id);

					int call_res = _param_getset_client.call(request.node_id, req);
					if (call_res < 0) {
						warnx("UAVCAN command bridge: couldn't send GetSet: %d", call_res);
					} else {
						_param_in_progress = true;
						_param_index = request.param_index;
					}
//.........这里部分代码省略.........

void MissionFeasibilityChecker::updateNavigationCapabilities()
{
	(void)orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
}