void estimator_base::gps_poll()
{
    _gps_new = false;

    orb_check(_gps_sub, &_gps_new);

    if (_gps_new) {
        orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps);
    }

    if (_gps.fix_type < 3 || !isfinite(_gps.lon) || !isfinite(_gps.lat) || !isfinite(_gps.alt) ) {
        _gps_new = false;
    }

    if (_gps_new && !_gps_init) {
        _gps_init = true;
        _init_lon = _gps.lon;
        _init_lat = _gps.lat;
        _init_alt = _gps.alt;
    }
}

void
MulticopterAttitudeControl::vehicle_status_poll()
{
	/* check if there is new status information */
	bool vehicle_status_updated;
	orb_check(_vehicle_status_sub, &vehicle_status_updated);

	if (vehicle_status_updated) {
		orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
		/* set correct uORB ID, depending on if vehicle is VTOL or not */
		if (!_rates_sp_id) {
			if (_vehicle_status.is_vtol) {
				_rates_sp_id = ORB_ID(mc_virtual_rates_setpoint);
				_actuators_id = ORB_ID(actuator_controls_virtual_mc);
			} else {
				_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
				_actuators_id = ORB_ID(actuator_controls_0);
			}
		}
	}
}

void
FixedwingPositionControl::vehicle_control_mode_poll()
{
	bool vstatus_updated;

	/* Check HIL state if vehicle status has changed */
	orb_check(_control_mode_sub, &vstatus_updated);

	if (vstatus_updated) {

		bool was_armed = _control_mode.flag_armed;

		orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);

		if (!was_armed && _control_mode.flag_armed) {
			_launch_lat = _global_pos.lat;
			_launch_lon = _global_pos.lon;
			_launch_alt = _global_pos.alt;
			_launch_valid = true;
		}
	}
}

void MulticopterLandDetector::updateParameterCache(const bool force)
{
	bool updated;
	parameter_update_s paramUpdate;

	orb_check(_parameterSub, &updated);

	if (updated) {
		orb_copy(ORB_ID(parameter_update), _parameterSub, &paramUpdate);
	}

	if (updated || force) {
		param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate);
		param_get(_paramHandle.maxVelocity, &_params.maxVelocity);
		param_get(_paramHandle.maxRotation, &_params.maxRotation_rad_s);
		_params.maxRotation_rad_s = math::radians(_params.maxRotation_rad_s);
		param_get(_paramHandle.maxThrottle, &_params.maxThrottle);
		param_get(_paramHandle.minManThrottle, &_params.minManThrottle);
		param_get(_paramHandle.acc_threshold_m_s2, &_params.acc_threshold_m_s2);
		param_get(_paramHandle.ff_trigger_time, &_params.ff_trigger_time);
	}
}

void OutputBase::_handle_position_update(bool force_update)
{
	bool need_update = force_update;

	if (!_cur_control_data || _cur_control_data->type != ControlData::Type::LonLat) {
		return;
	}

	if (!force_update) {
		orb_check(_vehicle_global_position_sub, &need_update);
	}

	if (!need_update) {
		return;
	}

	vehicle_global_position_s vehicle_global_position;
	orb_copy(ORB_ID(vehicle_global_position), _vehicle_global_position_sub, &vehicle_global_position);

	float pitch;
	const double &lon = _cur_control_data->type_data.lonlat.lon;
	const double &lat = _cur_control_data->type_data.lonlat.lat;
	const float &alt = _cur_control_data->type_data.lonlat.altitude;

	if (_cur_control_data->type_data.lonlat.pitch_fixed_angle >= -M_PI_F) {
		pitch = _cur_control_data->type_data.lonlat.pitch_fixed_angle;

	} else {
		pitch = _calculate_pitch(lon, lat, alt, vehicle_global_position);
	}

	float roll = _cur_control_data->type_data.lonlat.roll_angle;
	float yaw = get_bearing_to_next_waypoint(vehicle_global_position.lat, vehicle_global_position.lon, lat, lon)
		    - vehicle_global_position.yaw;

	_angle_setpoints[0] = roll;
	_angle_setpoints[1] = pitch + _cur_control_data->type_data.lonlat.pitch_angle_offset;
	_angle_setpoints[2] = yaw + _cur_control_data->type_data.lonlat.yaw_angle_offset;
}

bool
MavlinkOrbSubscription::is_published()
{
	// If we marked it as published no need to check again
	if (_published) {
		return true;
	}

	hrt_abstime now = hrt_absolute_time();

	if (now - _last_pub_check < 300000) {
		return false;
	}

	// We are checking now
	_last_pub_check = now;

	// We don't want to subscribe to anything that does not exist
	// in order to save memory and file descriptors.
	// However, for some topics like vehicle_command_ack, we want to subscribe
	// from the beginning in order not to miss or delay the first publish respective advertise.
	if (!_subscribe_from_beginning && orb_exists(_topic, _instance)) {
		return false;
	}

	if (_fd < 0) {
		_fd = orb_subscribe_multi(_topic, _instance);
	}

	bool updated;
	orb_check(_fd, &updated);

	if (updated) {
		_published = true;
	}

	return _published;
}

bool
MavlinkOrbSubscription::update_if_changed(void *data)
{
	bool prevpub = _published;

	if (!is_published()) {
		return false;
	}

	bool updated;

	if (orb_check(_fd, &updated)) {
		return false;
	}

	// If we didn't update and this topic did not change
	// its publication status then nothing really changed
	if (!updated && prevpub == _published) {
		return false;
	}

	return update(data);
}

void GPS::handleInjectDataTopic()
{
    if (_orb_inject_data_fd[0] == -1) {
        return;
    }

    bool updated = false;

    do {
        int orb_inject_data_cur_fd = _orb_inject_data_fd[_orb_inject_data_next];
        orb_check(orb_inject_data_cur_fd, &updated);

        if (updated) {
            struct gps_inject_data_s msg;
            orb_copy(ORB_ID(gps_inject_data), orb_inject_data_cur_fd, &msg);
            injectData(msg.data, msg.len);

            _orb_inject_data_next = (_orb_inject_data_next + 1) % _orb_inject_data_fd_count;

            ++_last_rate_rtcm_injection_count;
        }
    } while (updated);
}

int do_trim_calibration(int mavlink_fd)
{
	int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
	usleep(200000);
	struct manual_control_setpoint_s sp;
	bool changed;
	orb_check(sub_man, &changed);

	if (!changed) {
		mavlink_log_critical(mavlink_fd, "no inputs, aborting");
		return ERROR;
	}

	orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);

	/* set parameters */
	float p = sp.roll;
	param_set(param_find("TRIM_ROLL"), &p);
	p = sp.pitch;
	param_set(param_find("TRIM_PITCH"), &p);
	p = sp.yaw;
	param_set(param_find("TRIM_YAW"), &p);

	/* store to permanent storage */
	/* auto-save */
	int save_ret = param_save_default();

	if (save_ret != 0) {
		mavlink_log_critical(mavlink_fd, "TRIM: SAVE FAIL");
		close(sub_man);
		return ERROR;
	}

	mavlink_log_info(mavlink_fd, "trim cal done");
	close(sub_man);
	return OK;
}

void
MulticopterPositionControl::poll_subscriptions()
{
	bool updated;

	orb_check(_att_sub, &updated);

	if (updated) {
		orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
	}

	orb_check(_att_sp_sub, &updated);

	if (updated) {
		orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp);
	}

	orb_check(_control_mode_sub, &updated);

	if (updated) {
		orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
	}

	orb_check(_manual_sub, &updated);

	if (updated) {
		orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual);
	}

	orb_check(_arming_sub, &updated);

	if (updated) {
		orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming);
	}

	orb_check(_local_pos_sub, &updated);

	if (updated) {
		orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
	}

}

void Ekf2::task_main()
{
	// subscribe to relevant topics
	int sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
	int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
	int airspeed_sub = orb_subscribe(ORB_ID(airspeed));
	int params_sub = orb_subscribe(ORB_ID(parameter_update));
	int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
	int range_finder_sub = orb_subscribe(ORB_ID(distance_sensor));
	int ev_pos_sub = orb_subscribe(ORB_ID(vehicle_vision_position));
	int ev_att_sub = orb_subscribe(ORB_ID(vehicle_vision_attitude));
	int vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
	int status_sub = orb_subscribe(ORB_ID(vehicle_status));

	px4_pollfd_struct_t fds[2] = {};
	fds[0].fd = sensors_sub;
	fds[0].events = POLLIN;
	fds[1].fd = params_sub;
	fds[1].events = POLLIN;

	// initialise parameter cache
	updateParams();

	// initialize data structures outside of loop
	// because they will else not always be
	// properly populated
	sensor_combined_s sensors = {};
	vehicle_gps_position_s gps = {};
	airspeed_s airspeed = {};
	optical_flow_s optical_flow = {};
	distance_sensor_s range_finder = {};
	vehicle_land_detected_s vehicle_land_detected = {};
	vehicle_local_position_s ev_pos = {};
	vehicle_attitude_s ev_att = {};
	vehicle_status_s vehicle_status = {};

	while (!_task_should_exit) {
		int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000);

		if (ret < 0) {
			// Poll error, sleep and try again
			usleep(10000);
			continue;

		} else if (ret == 0) {
			// Poll timeout or no new data, do nothing
			continue;
		}

		if (fds[1].revents & POLLIN) {
			// read from param to clear updated flag
			struct parameter_update_s update;
			orb_copy(ORB_ID(parameter_update), params_sub, &update);
			updateParams();

			// fetch sensor data in next loop
			continue;

		} else if (!(fds[0].revents & POLLIN)) {
			// no new data
			continue;
		}

		bool gps_updated = false;
		bool airspeed_updated = false;
		bool optical_flow_updated = false;
		bool range_finder_updated = false;
		bool vehicle_land_detected_updated = false;
		bool vision_position_updated = false;
		bool vision_attitude_updated = false;
		bool vehicle_status_updated = false;

		orb_copy(ORB_ID(sensor_combined), sensors_sub, &sensors);
		// update all other topics if they have new data

		orb_check(status_sub, &vehicle_status_updated);

		if (vehicle_status_updated) {
			orb_copy(ORB_ID(vehicle_status), status_sub, &vehicle_status);
		}

		orb_check(gps_sub, &gps_updated);

		if (gps_updated) {
			orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps);
		}

		orb_check(airspeed_sub, &airspeed_updated);

		if (airspeed_updated) {
			orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed);
		}

		orb_check(optical_flow_sub, &optical_flow_updated);

		if (optical_flow_updated) {
			orb_copy(ORB_ID(optical_flow), optical_flow_sub, &optical_flow);
		}

		orb_check(range_finder_sub, &range_finder_updated);
//.........这里部分代码省略.........

//.........这里部分代码省略.........
						mavlink_log_info(mavlink_fd, "[inav] baro offset: %d m", (int)baro_offset);
						local_pos.z_valid = true;
						local_pos.v_z_valid = true;
					}
				}
			}
		}
	}

	/* main loop */
	px4_pollfd_struct_t fds[1] = {
		{ .fd = vehicle_attitude_sub, .events = POLLIN },
	};

	while (!thread_should_exit) {
		int ret = px4_poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate
		hrt_abstime t = hrt_absolute_time();

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

		} else if (ret > 0) {
			/* act on attitude updates */

			/* vehicle attitude */
			orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
			attitude_updates++;

			bool updated;

			/* parameter update */
			orb_check(parameter_update_sub, &updated);

			if (updated) {
				struct parameter_update_s update;
				orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
				inav_parameters_update(&pos_inav_param_handles, &params);
			}

			/* actuator */
			orb_check(actuator_sub, &updated);

			if (updated) {
				orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator);
			}

			/* armed */
			orb_check(armed_sub, &updated);

			if (updated) {
				orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
			}

			/* sensor combined */
			orb_check(sensor_combined_sub, &updated);

			if (updated) {
				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);

				if (sensor.accelerometer_timestamp != accel_timestamp) {
					if (att.R_valid) {
						/* correct accel bias */
						sensor.accelerometer_m_s2[0] -= acc_bias[0];
						sensor.accelerometer_m_s2[1] -= acc_bias[1];

//.........这里部分代码省略.........
		fds[1].fd = sub_params;
		fds[1].events = POLLIN;
		int ret = px4_poll(fds, 2, 1000);

		if (ret < 0) {
			/* XXX this is seriously bad - should be an emergency */
		} else if (ret == 0) {
			/* check if we're in HIL - not getting sensor data is fine then */
			orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode);

			if (!control_mode.flag_system_hil_enabled) {
				warnx("WARNING: Not getting sensor data - sensor app running?");
			}

		} else {

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

				/* update parameters */
				parameters_update(&ekf_param_handles, &ekf_params);
			}

			/* only run filter if sensor values changed */
			if (fds[0].revents & POLLIN) {

				/* get latest measurements */
				orb_copy(ORB_ID(sensor_combined), sub_raw, &raw);

				bool gps_updated;
				orb_check(sub_gps, &gps_updated);
				if (gps_updated) {
					orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps);

					if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp) < 1000000) {
						mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f));

						/* update mag declination rotation matrix */
						R_decl.from_euler(0.0f, 0.0f, mag_decl);
					}
				}

				bool global_pos_updated;
				orb_check(sub_global_pos, &global_pos_updated);
				if (global_pos_updated) {
					orb_copy(ORB_ID(vehicle_global_position), sub_global_pos, &global_pos);
				}

				if (!initialized) {
					// XXX disabling init for now
					initialized = true;

					// gyro_offsets[0] += raw.gyro_rad_s[0];
					// gyro_offsets[1] += raw.gyro_rad_s[1];
					// gyro_offsets[2] += raw.gyro_rad_s[2];
					// offset_count++;

					// if (hrt_absolute_time() - start_time > 3000000LL) {
					// 	initialized = true;
					// 	gyro_offsets[0] /= offset_count;
					// 	gyro_offsets[1] /= offset_count;
					// 	gyro_offsets[2] /= offset_count;
					// }

void
Delivery::to_destination()
{
	// set a mission to destination with takeoff enabled
	// Status = enroute ; change to Dropoff at completion

	if (_complete) {
		// Update status now that travel to destination is complete and reset _first_run for next stage
		_first_run = true;
		mavlink_log_critical(_navigator->get_mavlink_fd(), "Black Hawk at Location");
		advance_delivery();
		return;
	}

	check_mission_valid();

	/* check if anything has changed */
	bool onboard_updated = false;
	orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
	if (onboard_updated) {
		update_onboard_mission();
	}

	bool offboard_updated = false;
	orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);
	if (offboard_updated) {
		update_offboard_mission();
	}

	/* reset mission items if needed */
	if (onboard_updated || offboard_updated) {
		set_mission_items();
	}

	/* lets check if we reached the current mission item */
	if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
		set_mission_item_reached();
		if (_mission_item.autocontinue) {
			/* switch to next waypoint if 'autocontinue' flag set */
			advance_mission();
			set_mission_items();
			mavlink_log_critical(_navigator->get_mavlink_fd(), "Black Hawk advancing mission");
		}

	} else if (_mission_type != MISSION_TYPE_NONE &&_param_altmode.get() == MISSION_ALTMODE_FOH) {
		altitude_sp_foh_update();
	} else {
		/* if waypoint position reached allow loiter on the setpoint */
		if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) {
			_navigator->set_can_loiter_at_sp(true);
			_complete = true;
		}
	}

	/* see if we need to update the current yaw heading for rotary wing types */
	if (_navigator->get_vstatus()->is_rotary_wing 
			&& _param_yawmode.get() != MISSION_YAWMODE_NONE
			&& _mission_type != MISSION_TYPE_NONE) {
		heading_sp_update();
	}

}

void
BlinkM::led()
{

	static int vehicle_status_sub_fd;
	static int vehicle_gps_position_sub_fd;

	static int num_of_cells = 0;
	static int detected_cells_runcount = 0;

	static int t_led_color[8] = { 0, 0, 0, 0, 0, 0, 0, 0};
	static int t_led_blink = 0;
	static int led_thread_runcount=0;
	static int led_interval = 1000;

	static int no_data_vehicle_status = 0;
	static int no_data_vehicle_gps_position = 0;

	static bool topic_initialized = false;
	static bool detected_cells_blinked = false;
	static bool led_thread_ready = true;

	int num_of_used_sats = 0;

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

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

		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;
			}
			t_led_color[5] = LED_OFF;
			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_gps_position_s vehicle_gps_position_raw;

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

		bool new_data_vehicle_status;
		bool new_data_vehicle_gps_position;

		orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);

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

//.........这里部分代码省略.........
        //~ hrt_abstime t = hrt_absolute_time();
        bool updated;

        if (ret < 0) {
            /* XXX this is seriously bad - should be an emergency */
        } else if (ret == 0) {
            /* check if we're in HIL - not getting sensor data is fine then */
            orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode);

            if (!control_mode.flag_system_hil_enabled) {
                fprintf(stderr,
                        "[att ekf] WARNING: Not getting sensors - sensor app running?\n");
            }

        } else {

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

                /* update parameters */
                parameters_update(&ekf_param_handles, &ekf_params);
            }

            /* only run filter if sensor values changed */
            if (fds[0].revents & POLLIN) {

                /* get latest measurements */
                orb_copy(ORB_ID(sensor_combined), sub_raw, &raw);

                bool gps_updated;
                orb_check(sub_gps, &gps_updated);
                if (gps_updated) {
                    orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps);

                    if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) {
                        mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f));

                        /* update mag declination rotation matrix */
                        R_decl.from_euler(0.0f, 0.0f, mag_decl);
                    }
                }

                bool global_pos_updated;
                orb_check(sub_global_pos, &global_pos_updated);
                if (global_pos_updated) {
                    orb_copy(ORB_ID(vehicle_global_position), sub_global_pos, &global_pos);
                }

                if (!initialized) {
                    // XXX disabling init for now
                    initialized = true;

                    // gyro_offsets[0] += raw.gyro_rad_s[0];
                    // gyro_offsets[1] += raw.gyro_rad_s[1];
                    // gyro_offsets[2] += raw.gyro_rad_s[2];
                    // offset_count++;

                    // if (hrt_absolute_time() - start_time > 3000000LL) {
                    // 	initialized = true;
                    // 	gyro_offsets[0] /= offset_count;
                    // 	gyro_offsets[1] /= offset_count;
                    // 	gyro_offsets[2] /= offset_count;
                    // }

//.........这里部分代码省略.........
	flight_vector_e.autocontinue = true;
	flight_vector_s.altitude_is_relative = false;

	struct wind_estimate_s wind;

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

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

	// Whatever state the bay is in, we want it closed on startup
	lock_release();
	close_bay();

	while (!_task_should_exit) {

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

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

		/* vehicle commands updated */
		if (fds[0].revents & POLLIN) {
			orb_copy(ORB_ID(vehicle_command), _command_sub, &_command);
			handle_command(&_command);
		}

		orb_check(vehicle_global_position_sub, &updated);
		if (updated) {
			/* copy global position */
			orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos);
		}

		if (_global_pos.timestamp == 0) {
			continue;
		}

		const unsigned sleeptime_us = 9500;

		hrt_abstime last_run = hrt_absolute_time();
		float dt_runs = sleeptime_us / 1e6f;

		// switch to faster updates during the drop
		while (_drop_state > DROP_STATE_INIT) {

			// Get wind estimate
			orb_check(_wind_estimate_sub, &updated);
			if (updated) {
				orb_copy(ORB_ID(wind_estimate), _wind_estimate_sub, &wind);
			}

			// Get vehicle position
			orb_check(vehicle_global_position_sub, &updated);
			if (updated) {
				// copy global position
				orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos);
			}

			// Get parameter updates
			orb_check(parameter_update_sub, &updated);

int quad_commander_thread_main(int argc, char *argv[]) {
        warnx("[quad_commander] has begun");

        /**
         * Subscriptions
         */
        struct quad_swarm_cmd_s swarm_cmd;
        struct quad_mode_s state;
        struct vehicle_status_s v_status;

        memset(&swarm_cmd, 0, sizeof(swarm_cmd));
        memset(&state, 0, sizeof(state));
        memset(&v_status, 0, sizeof(v_status));

        int swarm_cmd_sub = 0;
        int state_sub = 0;
        int v_status_sub = 0;

        swarm_cmd_sub = orb_subscribe(ORB_ID(quad_swarm_cmd));
        state_sub = orb_subscribe(ORB_ID(quad_mode));
        v_status_sub = orb_subscribe(ORB_ID(vehicle_status));

        /**
         * Topics to be published on
         */
        struct quad_mode_s mode;
        orb_advert_t mode_pub = orb_advertise(ORB_ID(quad_mode), &mode);
        memset(&mode, 0, sizeof(mode));

	param_t param_ptr = param_find("MAV_SYS_ID");
	param_get(param_ptr, &system_id);

        mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
        mavlink_log_info(mavlink_fd, "[QC%i] started", system_id);

        struct pollfd fd_cmd[1];
        fd_cmd[0].fd = swarm_cmd_sub;
        fd_cmd[0].events = POLLIN;

        struct pollfd fd_state;
        fd_state.fd = state_sub;
        fd_state.events = POLLIN;

        /* Initial state of the quadrotor; operations always start from the ground */
        state.current_state = QUAD_STATE_GROUNDED;
        mode.current_state = QUAD_STATE_GROUNDED;
        orb_publish(ORB_ID(quad_mode), mode_pub, &mode);

        bool transition_error  = false;

        while ( !thread_should_exit ) {
                bool v_status_updated;
                orb_check(v_status_sub, &v_status_updated);
                if ( v_status_updated )
                        orb_copy(ORB_ID(vehicle_status), v_status_sub, &v_status);

                if ( v_status.arming_state == ARMING_STATE_STANDBY ) {
                        state.current_state = QUAD_STATE_GROUNDED;
                        mode.cmd = QUAD_CMD_PENDING;
                        mode.current_state = QUAD_STATE_GROUNDED;
                        orb_publish(ORB_ID(quad_mode), mode_pub, &mode);
                        swarm_cmd.cmd_id = (float)0;
                }

                if ( (v_status.battery_warning == VEHICLE_BATTERY_WARNING_LOW) &&
                     (state.current_state != QUAD_STATE_GROUNDED) ) {

                        low_battery = true;
                        mavlink_log_critical(mavlink_fd, "[QC%i] Battery lavel low!", system_id);
                        emergency_land( &state, &mode, &mode_pub, &state_sub, &swarm_cmd );

                } else {
                        low_battery = false;
                }

                int ret_state = poll(&fd_state, 1, 1);
                if ( ret_state < 0 ) {
                        warnx("poll cmd error");
                } else if ( ret_state == 0 ) {
                        /* nothing happened */
                } else if ( fd_state.revents & POLLIN ) {
                        orb_copy(ORB_ID(quad_mode), state_sub, &state);
                } else {
                        /* nothing happened */
                }

                if ( state.error == true ) {
                        int ret_value = emergency_land( &state, &mode, &mode_pub, &state_sub, &swarm_cmd );
                	mavlink_log_info(mavlink_fd, "[QC%i] emergency landing", system_id);
                        error_msg( ret_value, &transition_error );
                }

                int ret_cmd = poll(fd_cmd, 1, 250);
                if (ret_cmd < 0) {
                        warnx("poll cmd error");
                } else if (ret_cmd == 0) {
                        /* no return value - nothing has happened */
                } else if (fd_cmd[0].revents & POLLIN) {
                        orb_copy(ORB_ID(quad_swarm_cmd), swarm_cmd_sub, &swarm_cmd);

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

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
	 * else clear geofence data in datamanager */
	struct stat buffer;

	if (stat(GEOFENCE_FILENAME, &buffer) == 0) {
		warnx("Try to load geofence.txt");
		_geofence.loadFromFile(GEOFENCE_FILENAME);

	} else {
		if (_geofence.clearDm() != OK) {
			mavlink_log_critical(&_mavlink_log_pub, "failed clearing geofence");
		}
	}

	/* do subscriptions */
	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_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));
	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
	_home_pos_sub = orb_subscribe(ORB_ID(home_position));
	_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
	_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
	_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));

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

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

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

	bool global_pos_available_once = false;

	while (!_task_should_exit) {

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

		if (pret == 0) {
			/* timed out - periodic check for _task_should_exit, etc. */
			if (global_pos_available_once) {
				global_pos_available_once = false;
				PX4_WARN("navigator: global position timeout");
			}
			/* 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_WARN("nav: poll error %d, %d", pret, errno);
			continue;
		} else {
			/* success, global pos was available */
			global_pos_available_once = true;
		}

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

		/* sensors combined updated */
		orb_check(_sensor_combined_sub, &updated);
		if (updated) {
			sensor_combined_update();
		}

		/* parameters updated */
		orb_check(_param_update_sub, &updated);
		if (updated) {
//.........这里部分代码省略.........

int
nshterm_main(int argc, char *argv[])
{
    if (argc < 2) {
        printf("Usage: nshterm <device>\n");
        exit(1);
    }
    unsigned retries = 0;
    int fd = -1;
    int armed_fd = orb_subscribe(ORB_ID(actuator_armed));
    struct actuator_armed_s armed;
    /* we assume the system does not provide arming status feedback */
    bool armed_updated = false;

    /* try the first 30 seconds or if arming system is ready */
    while ((retries < 300) || armed_updated) {

        /* abort if an arming topic is published and system is armed */
        bool updated = false;
        if (orb_check(armed_fd, &updated)) {
            /* the system is now providing arming status feedback.
             * instead of timing out, we resort to abort bringing
             * up the terminal.
             */
            armed_updated = true;
            orb_copy(ORB_ID(actuator_armed), armed_fd, &armed);

            if (armed.armed) {
                /* this is not an error, but we are done */
                exit(0);
            }
        }

        /* the retries are to cope with the behaviour of /dev/ttyACM0 */
        /* which may not be ready immediately. */
        fd = open(argv[1], O_RDWR);
        if (fd != -1) {
            break;
        }
        usleep(100000);
        retries++;
    }
    if (fd == -1) {
        perror(argv[1]);
        exit(1);
    }

    /* set up the serial port with output processing */
    
    /* Try to set baud rate */
    struct termios uart_config;
    int termios_state;

    /* Back up the original uart configuration to restore it after exit */
    if ((termios_state = tcgetattr(fd, &uart_config)) < 0) {
        warnx("ERR get config %s: %d\n", argv[1], termios_state);
        close(fd);
        return -1;
    }

    /* Set ONLCR flag (which appends a CR for every LF) */
    uart_config.c_oflag |= (ONLCR | OPOST/* | OCRNL*/);

    if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
        warnx("ERR set config %s\n", argv[1]);
        close(fd);
        return -1;
    }

    /* setup standard file descriptors */
    close(0);
    close(1);
    close(2);
    dup2(fd, 0);
    dup2(fd, 1);
    dup2(fd, 2);

    nsh_consolemain(0, NULL);

    close(fd);

    return OK;
}