C++ orb_subscribe函数代码示例

int do_airspeed_calibration(int mavlink_fd)
{
	/* give directions */
	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);

	const unsigned calibration_count = 2000;

	int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
	struct differential_pressure_s diff_pres;

	float diff_pres_offset = 0.0f;

	/* Reset sensor parameters */
	struct airspeed_scale airscale = {
		diff_pres_offset,
		1.0f,
	};

	bool paramreset_successful = false;
	int  fd = open(AIRSPEED_DEVICE_PATH, 0);

	if (fd > 0) {
		if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
			paramreset_successful = true;

		} else {
			mavlink_log_critical(mavlink_fd, "airspeed offset zero failed");
		}

		close(fd);
	}

	if (!paramreset_successful) {

		/* only warn if analog scaling is zero */
		float analog_scaling = 0.0f;
		param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
		if (fabsf(analog_scaling) < 0.1f) {
			mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]");
			close(diff_pres_sub);
			return ERROR;
		}

		/* set scaling offset parameter */
		if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
			mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
			close(diff_pres_sub);
			return ERROR;
		}
	}

	unsigned calibration_counter = 0;

	mavlink_log_critical(mavlink_fd, "Ensure sensor is not measuring wind");
	usleep(500 * 1000);

	while (calibration_counter < calibration_count) {

		/* wait blocking for new data */
		struct pollfd fds[1];
		fds[0].fd = diff_pres_sub;
		fds[0].events = POLLIN;

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

		if (poll_ret) {
			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);

			diff_pres_offset += diff_pres.differential_pressure_raw_pa;
			calibration_counter++;

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

		} else if (poll_ret == 0) {
			/* any poll failure for 1s is a reason to abort */
			mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
			close(diff_pres_sub);
			return ERROR;
		}
	}

	diff_pres_offset = diff_pres_offset / calibration_count;

	if (isfinite(diff_pres_offset)) {

		int  fd_scale = open(AIRSPEED_DEVICE_PATH, 0);
		airscale.offset_pa = diff_pres_offset;
		if (fd_scale > 0) {
			if (OK != ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
				mavlink_log_critical(mavlink_fd, "airspeed offset update failed");
			}

			close(fd_scale);
		}

		if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
			mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
			close(diff_pres_sub);
//.........这里部分代码省略.........

void AttitudePositionEstimatorEKF::task_main()
{
	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);

	_ekf = new AttPosEKF();

	_filter_start_time = hrt_absolute_time();

	if (!_ekf) {
		warnx("OUT OF MEM!");
		return;
	}

	/*
	 * do subscriptions
	 */
	_distance_sub = orb_subscribe(ORB_ID(distance_sensor));
	_baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0);
	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
	_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
	_params_sub = orb_subscribe(ORB_ID(parameter_update));
	_home_sub = orb_subscribe(ORB_ID(home_position));
	_landDetectorSub = orb_subscribe(ORB_ID(vehicle_land_detected));
	_armedSub = orb_subscribe(ORB_ID(actuator_armed));

	/* rate limit vehicle status updates to 5Hz */
	orb_set_interval(_vstatus_sub, 200);

	_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
	/* XXX remove this!, BUT increase the data buffer size! */
	orb_set_interval(_sensor_combined_sub, 9);

	/* sets also parameters in the EKF object */
	parameters_update();

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

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

	fds[1].fd = _sensor_combined_sub;
	fds[1].events = POLLIN;

	_gps.vel_n_m_s = 0.0f;
	_gps.vel_e_m_s = 0.0f;
	_gps.vel_d_m_s = 0.0f;

	_task_running = true;

	while (!_task_should_exit) {

		/* wait for up to 100ms for data */
		int pret = px4_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 ERR %d, %d", pret, errno);
			continue;
		}

		perf_begin(_loop_perf);
		perf_count(_loop_intvl);

		/* 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 estimator if gyro updated */
		if (fds[1].revents & POLLIN) {

			/* check vehicle status for changes to publication state */
			bool prev_hil = (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON);
			vehicle_status_poll();

			perf_count(_perf_gyro);

			/* Reset baro reference if switching to HIL, reset sensor states */
			if (!prev_hil && (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON)) {
				/* system is in HIL now, wait for measurements to come in one last round */
				usleep(60000);

				/* now read all sensor publications to ensure all real sensor data is purged */
				orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);

				/* set sensors to de-initialized state */
				_gyro_valid = false;
//.........这里部分代码省略.........

int do_airspeed_calibration(int mavlink_fd)
{
	int result = OK;
	unsigned calibration_counter = 0;
	const unsigned maxcount = 3000;

	/* give directions */
	mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);

	const unsigned calibration_count = 2000;

	int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
	struct differential_pressure_s diff_pres;

	float diff_pres_offset = 0.0f;

	/* Reset sensor parameters */
	struct airspeed_scale airscale = {
		diff_pres_offset,
		1.0f,
	};

	bool paramreset_successful = false;
	int  fd = px4_open(AIRSPEED0_DEVICE_PATH, 0);

	if (fd > 0) {
		if (OK == px4_ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
			paramreset_successful = true;

		} else {
			mavlink_log_critical(mavlink_fd, "[cal] airspeed offset zero failed");
		}

		px4_close(fd);
	}
    
	int cancel_sub = calibrate_cancel_subscribe();

	if (!paramreset_successful) {

		/* only warn if analog scaling is zero */
		float analog_scaling = 0.0f;
		param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
		if (fabsf(analog_scaling) < 0.1f) {
			mavlink_log_critical(mavlink_fd, "[cal] No airspeed sensor, see http://px4.io/help/aspd");
			goto error_return;
		}

		/* set scaling offset parameter */
		if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
			mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG);
			goto error_return;
		}
	}

	mavlink_log_critical(mavlink_fd, "[cal] Ensure sensor is not measuring wind");
	usleep(500 * 1000);

	while (calibration_counter < calibration_count) {

		if (calibrate_cancel_check(mavlink_fd, cancel_sub)) {
			goto error_return;
		}
        
		/* wait blocking for new data */
		px4_pollfd_struct_t fds[1];
		fds[0].fd = diff_pres_sub;
		fds[0].events = POLLIN;

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

		if (poll_ret) {
			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);

			diff_pres_offset += diff_pres.differential_pressure_raw_pa;
			calibration_counter++;

			if (calibration_counter % (calibration_count / 20) == 0) {
				mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count);
			}

		} else if (poll_ret == 0) {
			/* any poll failure for 1s is a reason to abort */
			feedback_calibration_failed(mavlink_fd);
			goto error_return;
		}
	}

	diff_pres_offset = diff_pres_offset / calibration_count;

	if (PX4_ISFINITE(diff_pres_offset)) {

		int  fd_scale = px4_open(AIRSPEED0_DEVICE_PATH, 0);
		airscale.offset_pa = diff_pres_offset;
		if (fd_scale > 0) {
			if (OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
				mavlink_log_critical(mavlink_fd, "[cal] airspeed offset update failed");
			}

			px4_close(fd_scale);
//.........这里部分代码省略.........

int do_level_calibration(orb_advert_t *mavlink_log_pub) {
	const unsigned cal_time = 5;
	const unsigned cal_hz = 100;
	unsigned settle_time = 30;

	bool success = false;
	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
	struct vehicle_attitude_s att;
	memset(&att, 0, sizeof(att));

	calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "level");

	param_t roll_offset_handle = param_find("SENS_BOARD_X_OFF");
	param_t pitch_offset_handle = param_find("SENS_BOARD_Y_OFF");
	param_t board_rot_handle = param_find("SENS_BOARD_ROT");

	// save old values if calibration fails
	float roll_offset_current;
	float pitch_offset_current;
	int32_t board_rot_current = 0;
	param_get(roll_offset_handle, &roll_offset_current);
	param_get(pitch_offset_handle, &pitch_offset_current);
	param_get(board_rot_handle, &board_rot_current);

	// give attitude some time to settle if there have been changes to the board rotation parameters
	if (board_rot_current == 0 && fabsf(roll_offset_current) < FLT_EPSILON && fabsf(pitch_offset_current) < FLT_EPSILON ) {
		settle_time = 0;
	}

	float zero = 0.0f;
	param_set_no_notification(roll_offset_handle, &zero);
	param_set_no_notification(pitch_offset_handle, &zero);
	param_notify_changes();

	px4_pollfd_struct_t fds[1];

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

	float roll_mean = 0.0f;
	float pitch_mean = 0.0f;
	unsigned counter = 0;

	// sleep for some time
	hrt_abstime start = hrt_absolute_time();
	while(hrt_elapsed_time(&start) < settle_time * 1000000) {
		calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (int)(90*hrt_elapsed_time(&start)/1e6f/(float)settle_time));
		sleep(settle_time / 10);
	}

	start = hrt_absolute_time();
	// average attitude for 5 seconds
	while(hrt_elapsed_time(&start) < cal_time * 1000000) {
		int pollret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);

		if (pollret <= 0) {
			// attitude estimator is not running
			calibration_log_critical(mavlink_log_pub, "attitude estimator not running - check system boot");
			calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "level");
			goto out;
		}

		orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
		matrix::Eulerf euler = matrix::Quatf(att.q);
		roll_mean += euler.phi();
		pitch_mean += euler.theta();
		counter++;
	}

	calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100);

	if (counter > (cal_time * cal_hz / 2 )) {
		roll_mean /= counter;
		pitch_mean /= counter;
	} else {
		calibration_log_info(mavlink_log_pub, "not enough measurements taken");
		success = false;
		goto out;
	}

	if (fabsf(roll_mean) > 0.8f ) {
		calibration_log_critical(mavlink_log_pub, "excess roll angle");
	} else if (fabsf(pitch_mean) > 0.8f ) {
		calibration_log_critical(mavlink_log_pub, "excess pitch angle");
	} else {
		roll_mean *= (float)M_RAD_TO_DEG;
		pitch_mean *= (float)M_RAD_TO_DEG;
		param_set_no_notification(roll_offset_handle, &roll_mean);
		param_set_no_notification(pitch_offset_handle, &pitch_mean);
		param_notify_changes();
		success = true;
	}

out:
	if (success) {
		calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "level");
		return 0;
	} else {
		// set old parameters
		param_set_no_notification(roll_offset_handle, &roll_offset_current);
//.........这里部分代码省略.........

void
PX4FMU::cycle()
{
	if (!_initialized) {

		/* 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));

		/* initialize PWM limit lib */
		pwm_limit_init(&_pwm_limit);

		update_pwm_rev_mask();

#ifdef RC_SERIAL_PORT
		_sbus_fd = sbus_init(RC_SERIAL_PORT, true);
#endif
		_initialized = true;
	}

	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\n", 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;
	}

	/* check if anything updated */
    //从mkblctrl-blctrl电子模块驱动拿数据，貌似没用到，而且poll里面也在等待定时器导致定时器卡死
	int ret = 0;//::poll(_poll_fds, _poll_fds_num, 0);

	/* this would be bad... */
	if (ret < 0) {
		DEVICE_LOG("poll error %d\n", ret);

	} else if (ret == 0) {
		/* timeout: no control data, switch to failsafe values */
//			warnx("no PWM: failsafe\n");

	} 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 != NULL) {

			size_t num_outputs;

			switch (_mode) {
			case MODE_2PWM:
				num_outputs = 2;
				break;

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

void
MulticopterAttitudeControl::task_main()
{

	/*
	 * 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));
	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
	_motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits));

	/* 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();
			vehicle_status_poll();
			vehicle_motor_limits_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(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);

				} else if (_rates_sp_id) {
					_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_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 = math::min(_manual_control_sp.z, MANUAL_THROTTLE_MAX_MULTICOPTER);

					/* 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();
//.........这里部分代码省略.........

//.........这里部分代码省略.........
		matrix::Vector3f accel_offs_vec(accel_offs[uorb_index]);
		matrix::Vector3f accel_offs_rotated = board_rotation_t * accel_offs_vec;
		matrix::Matrix3f accel_T_mat(accel_T[uorb_index]);
		matrix::Matrix3f 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) {
						val += accel_scale.x_offset;
					} else if (axis_index == 1) {
						val += accel_scale.y_offset;
					} else if (axis_index == 2) {
						val += accel_scale.z_offset;
					}
					failed |= (PX4_OK != param_set_no_notification(handle, &val));
				}

				/* update the _SCL_ terms to include the scale factor */
				for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
					val = 1.0f;
					(void)sprintf(str, "TC_A%u_SCL_%u", sensor_correction.accel_mapping[uorb_index], axis_index);
					handle = param_find(str);
					if (axis_index == 0) {
						val = accel_scale.x_scale;

void
FixedwingEstimator::task_main()
{
	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);

	_ekf = new AttPosEKF();
	float dt = 0.0f; // time lapsed since last covariance prediction
	_filter_start_time = hrt_absolute_time();

	if (!_ekf) {
		errx(1, "OUT OF MEM!");
	}

	/*
	 * do subscriptions
	 */
	_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
	_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
	_params_sub = orb_subscribe(ORB_ID(parameter_update));
	_home_sub = orb_subscribe(ORB_ID(home_position));

	/* rate limit vehicle status updates to 5Hz */
	orb_set_interval(_vstatus_sub, 200);

#ifndef SENSOR_COMBINED_SUB

	_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
	_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
	_mag_sub = orb_subscribe(ORB_ID(sensor_mag));

	/* rate limit gyro updates to 50 Hz */
	/* XXX remove this!, BUT increase the data buffer size! */
	orb_set_interval(_gyro_sub, 4);
#else
	_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
	/* XXX remove this!, BUT increase the data buffer size! */
	orb_set_interval(_sensor_combined_sub, 9);
#endif

	/* sets also parameters in the EKF object */
	parameters_update();

	Vector3f lastAngRate;
	Vector3f lastAccel;

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

	/* Setup of loop */
	fds[0].fd = _params_sub;
	fds[0].events = POLLIN;
#ifndef SENSOR_COMBINED_SUB
	fds[1].fd = _gyro_sub;
	fds[1].events = POLLIN;
#else
	fds[1].fd = _sensor_combined_sub;
	fds[1].events = POLLIN;
#endif

	bool newDataGps = false;
	bool newHgtData = false;
	bool newAdsData = false;
	bool newDataMag = false;

	float posNED[3] = {0.0f, 0.0f, 0.0f}; // North, East Down position (m)

	uint64_t last_gps = 0;
	_gps.vel_n_m_s = 0.0f;
	_gps.vel_e_m_s = 0.0f;
	_gps.vel_d_m_s = 0.0f;

	while (!_task_should_exit) {

		/* 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 ERR %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();
		}

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

/****************************************************************************
 * main
 ****************************************************************************/
int position_estimator_inav_thread_main(int argc, char *argv[])
{
	warnx("started");
	int mavlink_fd;
	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
	mavlink_log_info(mavlink_fd, "[inav] started");

	float x_est[3] = { 0.0f, 0.0f, 0.0f };
	float y_est[3] = { 0.0f, 0.0f, 0.0f };
	float z_est[3] = { 0.0f, 0.0f, 0.0f };

	int baro_init_cnt = 0;
	int baro_init_num = 200;
	float baro_offset = 0.0f;		// baro offset for reference altitude, initialized on start, then adjusted
	float surface_offset = 0.0f;	// ground level offset from reference altitude
	float surface_offset_rate = 0.0f;	// surface offset change rate
	float alt_avg = 0.0f;
	bool landed = true;
	hrt_abstime landed_time = 0;

	hrt_abstime accel_timestamp = 0;
	hrt_abstime baro_timestamp = 0;

	bool ref_inited = false;
	hrt_abstime ref_init_start = 0;
	const hrt_abstime ref_init_delay = 1000000;	// wait for 1s after 3D fix

	uint16_t accel_updates = 0;
	uint16_t baro_updates = 0;
	uint16_t gps_updates = 0;
	uint16_t attitude_updates = 0;
	uint16_t flow_updates = 0;

	hrt_abstime updates_counter_start = hrt_absolute_time();
	hrt_abstime pub_last = hrt_absolute_time();

	hrt_abstime t_prev = 0;

	/* acceleration in NED frame */
	float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G };

	/* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */
	float corr_acc[] = { 0.0f, 0.0f, 0.0f };	// N E D
	float acc_bias[] = { 0.0f, 0.0f, 0.0f };	// body frame
	float corr_baro = 0.0f;		// D
	float corr_gps[3][2] = {
		{ 0.0f, 0.0f },		// N (pos, vel)
		{ 0.0f, 0.0f },		// E (pos, vel)
		{ 0.0f, 0.0f },		// D (pos, vel)
	};
	float w_gps_xy = 1.0f;
	float w_gps_z = 1.0f;
	float corr_sonar = 0.0f;
	float corr_sonar_filtered = 0.0f;

	float corr_flow[] = { 0.0f, 0.0f };	// N E
	float w_flow = 0.0f;

	float sonar_prev = 0.0f;
	hrt_abstime sonar_time = 0;			// time of last sonar measurement (not filtered)
	hrt_abstime sonar_valid_time = 0;	// time of last sonar measurement used for correction (filtered)
	hrt_abstime xy_src_time = 0;		// time of last available position data

	bool gps_valid = false;			// GPS is valid
	bool sonar_valid = false;		// sonar is valid
	bool flow_valid = false;		// flow is valid
	bool flow_accurate = false;		// flow should be accurate (this flag not updated if flow_valid == false)

	/* declare and safely initialize all structs */
	struct actuator_controls_s actuator;
	memset(&actuator, 0, sizeof(actuator));
	struct actuator_armed_s armed;
	memset(&armed, 0, sizeof(armed));
	struct sensor_combined_s sensor;
	memset(&sensor, 0, sizeof(sensor));
	struct vehicle_gps_position_s gps;
	memset(&gps, 0, sizeof(gps));
	struct vehicle_attitude_s att;
	memset(&att, 0, sizeof(att));
	struct vehicle_local_position_s local_pos;
	memset(&local_pos, 0, sizeof(local_pos));
	struct optical_flow_s flow;
	memset(&flow, 0, sizeof(flow));
	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));

	/* advertise */
	orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);
//.........这里部分代码省略.........

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
PX4FMU::task_main()
{
	/* force a reset of the update rate */
	_current_update_rate = 0;

	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));

	/* advertise the mixed control outputs */
	actuator_outputs_s outputs;
	memset(&outputs, 0, sizeof(outputs));

#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);

	log("starting");

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

			debug("adjusted actuator update interval to %ums", update_rate_in_ms);
			for (unsigned i = 0; i < 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) {
			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 < 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) {

				unsigned num_outputs;

				switch (_mode) {
//.........这里部分代码省略.........

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

	/* 45 seconds */
	uint64_t calibration_interval = 45 * 1000 * 1000;

	/* maximum 500 values */
	const unsigned int calibration_maxcount = 500;
	unsigned int calibration_counter;

	struct mag_scale mscale_null = {
		0.0f,
		1.0f,
		0.0f,
		1.0f,
		0.0f,
		1.0f,
	};

	int res = OK;

	/* erase old calibration */
	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
	res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);

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

	if (res == OK) {
		/* calibrate range */
		res = ioctl(fd, MAGIOCCALIBRATE, fd);

		if (res != OK) {
			mavlink_log_critical(mavlink_fd, "Skipped scale calibration");
			/* this is non-fatal - mark it accordingly */
			res = OK;
		}
	}

	close(fd);

	float *x = NULL;
	float *y = NULL;
	float *z = NULL;

	if (res == OK) {
		/* allocate memory */
		mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20);

		x = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
		y = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
		z = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));

		if (x == NULL || y == NULL || z == NULL) {
			mavlink_log_critical(mavlink_fd, "ERROR: out of memory");
			res = ERROR;
			return res;
		}
	} else {
		/* exit */
		return ERROR;
	}

	if (res == OK) {
		int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
		struct mag_report mag;

		/* limit update rate to get equally spaced measurements over time (in ms) */
		orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount);

		/* calibrate offsets */
		uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
		unsigned poll_errcount = 0;

		mavlink_log_info(mavlink_fd, "rotate in a figure 8 around all axis");

		calibration_counter = 0;

		while (hrt_absolute_time() < calibration_deadline &&
		       calibration_counter < calibration_maxcount) {

			/* wait blocking for new data */
			struct pollfd fds[1];
			fds[0].fd = sub_mag;
			fds[0].events = POLLIN;

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

			if (poll_ret > 0) {
				orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);

				x[calibration_counter] = mag.x;
				y[calibration_counter] = mag.y;
				z[calibration_counter] = mag.z;

				calibration_counter++;

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

void
Gimbal::cycle()
{
	if (!_initialized) {
		/* get a subscription handle on the vehicle command topic */
		_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));

		/* get a publication handle on actuator output topic */
		struct actuator_controls_s zero_report;
		memset(&zero_report, 0, sizeof(zero_report));
		zero_report.timestamp = hrt_absolute_time();
		_actuator_controls_2_topic = orb_advertise(ORB_ID(actuator_controls_2), &zero_report);

		if (_actuator_controls_2_topic == nullptr) {
			warnx("advert err");
		}

		_initialized = true;
	}

	bool	updated = false;

	perf_begin(_sample_perf);

	float roll = 0.0f;
	float pitch = 0.0f;
	float yaw = 0.0f;


	if (_att_sub < 0) {
		_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
	}

	vehicle_attitude_s att;

	orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att);

	if (_attitude_compensation_roll) {
		roll = 1.0f / M_PI_F * -att.roll;
		updated = true;
	}

	if (_attitude_compensation_pitch) {
		pitch = 1.0f / M_PI_F * -att.pitch;
		updated = true;
	}

	if (_attitude_compensation_yaw) {
		yaw = 1.0f / M_PI_F * att.yaw;
		updated = true;
	}


	struct vehicle_command_s cmd;

	bool cmd_updated;

	orb_check(_vehicle_command_sub, &cmd_updated);

	if (cmd_updated) {

		orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);

		if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL
				|| cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT) {

			_control_cmd = cmd;
			_control_cmd_set = true;

		} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE) {

			_config_cmd = cmd;
			_config_cmd_set = true;

		}

	}

	if (_config_cmd_set) {

		_config_cmd_set = false;

		_attitude_compensation_roll = (int)_config_cmd.param2 == 1;
		_attitude_compensation_pitch = (int)_config_cmd.param3 == 1;
		_attitude_compensation_yaw = (int)_config_cmd.param4 == 1;

	}

	if (_control_cmd_set) {

		unsigned mountMode = _control_cmd.param7;
		DEVICE_DEBUG("control_cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", _control_cmd.command, mountMode, (double)_control_cmd.param1, (double)_control_cmd.param2);

		if (_control_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL &&
			mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) {
			/* Convert to range -1 ... 1, which corresponds to -180deg ... 180deg */
			roll += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param1;
			pitch += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param2;
			yaw += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param3;
			
//.........这里部分代码省略.........

int ardrone_interface_thread_main(int argc, char *argv[])
{
	thread_running = true;

	char *device = "/dev/ttyS1";

	/* File descriptors */
	int gpios;

	char *commandline_usage = "\tusage: ardrone_interface start|status|stop [-t for motor test (10%% thrust)]\n";

	bool motor_test_mode = false;
	int test_motor = -1;

	/* read commandline arguments */
	for (int i = 0; i < argc && argv[i]; i++) {
		if (strcmp(argv[i], "-t") == 0 || strcmp(argv[i], "--test") == 0) {
			motor_test_mode = true;
		}

		if (strcmp(argv[i], "-m") == 0 || strcmp(argv[i], "--motor") == 0) {
			if (i + 1 < argc) {
				int motor = atoi(argv[i + 1]);

				if (motor > 0 && motor < 5) {
					test_motor = motor;

				} else {
					thread_running = false;
					errx(1, "supply a motor # between 1 and 4. Example: -m 1\n %s", commandline_usage);
				}

			} else {
				thread_running = false;
				errx(1, "missing parameter to -m 1..4\n %s", commandline_usage);
			}
		}

		if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set
			if (argc > i + 1) {
				device = argv[i + 1];

			} else {
				thread_running = false;
				errx(1, "missing parameter to -m 1..4\n %s", commandline_usage);
			}
		}
	}

	struct termios uart_config_original;

	if (motor_test_mode) {
		warnx("setting 10 %% thrust.\n");
	}

	/* Led animation */
	int counter = 0;
	int led_counter = 0;

	/* declare and safely initialize all structs */
	struct actuator_controls_s actuator_controls;
	memset(&actuator_controls, 0, sizeof(actuator_controls));
	struct actuator_armed_s armed;
	//XXX is this necessairy?
	armed.armed = false;

	/* subscribe to attitude, motor setpoints and system state */
	int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));

	/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
	ardrone_write = ardrone_open_uart(device, &uart_config_original);

	/* initialize multiplexing, deactivate all outputs - must happen after UART open to claim GPIOs on PX4FMU */
	gpios = ar_multiplexing_init();

	if (ardrone_write < 0) {
		warnx("No UART, exiting.");
		thread_running = false;
		exit(ERROR);
	}

	/* initialize motors */
	if (OK != ar_init_motors(ardrone_write, gpios)) {
		close(ardrone_write);
		warnx("motor init fail");
		thread_running = false;
		exit(ERROR);
	}

	ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0);


	// XXX Re-done initialization to make sure it is accepted by the motors
	// XXX should be removed after more testing, but no harm

	/* close uarts */
	close(ardrone_write);

	/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
//.........这里部分代码省略.........

void
BlinkM::led()
{

	if(!topic_initialized) {
		vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
		orb_set_interval(vehicle_status_sub_fd, 250);

		vehicle_control_mode_sub_fd = orb_subscribe(ORB_ID(vehicle_control_mode));
		orb_set_interval(vehicle_control_mode_sub_fd, 250);

		actuator_armed_sub_fd = orb_subscribe(ORB_ID(actuator_armed));
		orb_set_interval(actuator_armed_sub_fd, 250);

		vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position));
		orb_set_interval(vehicle_gps_position_sub_fd, 250);

		/* Subscribe to safety topic */
		safety_sub_fd = orb_subscribe(ORB_ID(safety));
		orb_set_interval(safety_sub_fd, 250);

		topic_initialized = true;
	}

	if(led_thread_ready == true) {
		if(!detected_cells_blinked) {
			if(num_of_cells > 0) {
				t_led_color[0] = LED_PURPLE;
			}
			if(num_of_cells > 1) {
				t_led_color[1] = LED_PURPLE;
			}
			if(num_of_cells > 2) {
				t_led_color[2] = LED_PURPLE;
			}
			if(num_of_cells > 3) {
				t_led_color[3] = LED_PURPLE;
			}
			if(num_of_cells > 4) {
				t_led_color[4] = LED_PURPLE;
			}
			if(num_of_cells > 5) {
				t_led_color[5] = LED_PURPLE;
			}
			t_led_color[6] = LED_OFF;
			t_led_color[7] = LED_OFF;
			t_led_blink = LED_BLINK;
		} else {
			t_led_color[0] = led_color_1;
			t_led_color[1] = led_color_2;
			t_led_color[2] = led_color_3;
			t_led_color[3] = led_color_4;
			t_led_color[4] = led_color_5;
			t_led_color[5] = led_color_6;
			t_led_color[6] = led_color_7;
			t_led_color[7] = led_color_8;
			t_led_blink = led_blink;
		}
		led_thread_ready = false;
	}

	if (led_thread_runcount & 1) {
		if (t_led_blink)
			setLEDColor(LED_OFF);
		led_interval = LED_OFFTIME;
	} else {
		setLEDColor(t_led_color[(led_thread_runcount / 2) % 8]);
		//led_interval = (led_thread_runcount & 1) : LED_ONTIME;
		led_interval = LED_ONTIME;
	}

	if (led_thread_runcount == 15) {
		/* obtained data for the first file descriptor */
		struct vehicle_status_s vehicle_status_raw;
		struct vehicle_control_mode_s vehicle_control_mode;
		struct actuator_armed_s actuator_armed;
		struct vehicle_gps_position_s vehicle_gps_position_raw;
		struct safety_s safety;

		memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw));
		memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw));
		memset(&safety, 0, sizeof(safety));

		bool new_data_vehicle_status;
		bool new_data_vehicle_control_mode;
		bool new_data_actuator_armed;
		bool new_data_vehicle_gps_position;
		bool new_data_safety;

		orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);

		int no_data_vehicle_status = 0;
		int no_data_vehicle_control_mode = 0;
		int no_data_actuator_armed = 0;
		int no_data_vehicle_gps_position = 0;

		if (new_data_vehicle_status) {
			orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw);
			no_data_vehicle_status = 0;
		} else {
//.........这里部分代码省略.........