C++ ECL_RollController类代码示例

本文整理汇总了C++中ECL_RollController类的典型用法代码示例。如果您正苦于以下问题：C++ ECL_RollController类的具体用法？C++ ECL_RollController怎么用？C++ ECL_RollController使用的例子？那么, 这里精选的类代码示例或许可以为您提供帮助。

在下文中一共展示了ECL_RollController类的6个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。

示例1: q_att

void
FixedwingAttitudeControl::task_main()
{
	/*
	 * do subscriptions
	 */
	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
	_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
	_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));
	_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
	_battery_status_sub = orb_subscribe(ORB_ID(battery_status));

	parameters_update();

	/* get an initial update for all sensor and status data */
	vehicle_setpoint_poll();
	vehicle_control_mode_poll();
	vehicle_manual_poll();
	vehicle_status_poll();
	vehicle_land_detected_poll();
	battery_status_poll();
	_sub_airspeed.update();

	/* wakeup source */
	px4_pollfd_struct_t fds[1];

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

	_task_running = true;

	while (!_task_should_exit) {
		static int loop_counter = 0;

		/* wait for up to 500ms 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) {
			PX4_WARN("poll error %d, %d", pret, errno);
			continue;
		}

		perf_begin(_loop_perf);

		/* only update parameters if they changed */
		bool params_updated = false;
		orb_check(_params_sub, &params_updated);

		if (params_updated) {
			/* read from param to clear updated flag */
			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[0].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);

			/* get current rotation matrix and euler angles from control state quaternions */
			math::Quaternion q_att(_att.q[0], _att.q[1], _att.q[2], _att.q[3]);
			_R = q_att.to_dcm();

			math::Vector<3> euler_angles;
			euler_angles = _R.to_euler();
			_roll    = euler_angles(0);
			_pitch   = euler_angles(1);
			_yaw     = euler_angles(2);

			if (_vehicle_status.is_vtol && _parameters.vtol_type == vtol_type::TAILSITTER) {
				/* 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.
//.........这里部分代码省略.........

开发者ID:larics，项目名称:Firmware，代码行数:101，代码来源:fw_att_control_main.cpp

示例2:

int
FixedwingAttitudeControl::parameters_update()
{

	param_get(_parameter_handles.p_tc, &(_parameters.p_tc));
	param_get(_parameter_handles.p_p, &(_parameters.p_p));
	param_get(_parameter_handles.p_i, &(_parameters.p_i));
	param_get(_parameter_handles.p_ff, &(_parameters.p_ff));
	param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos));
	param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg));
	param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max));

	param_get(_parameter_handles.r_tc, &(_parameters.r_tc));
	param_get(_parameter_handles.r_p, &(_parameters.r_p));
	param_get(_parameter_handles.r_i, &(_parameters.r_i));
	param_get(_parameter_handles.r_ff, &(_parameters.r_ff));

	param_get(_parameter_handles.r_integrator_max, &(_parameters.r_integrator_max));
	param_get(_parameter_handles.r_rmax, &(_parameters.r_rmax));

	param_get(_parameter_handles.y_p, &(_parameters.y_p));
	param_get(_parameter_handles.y_i, &(_parameters.y_i));
	param_get(_parameter_handles.y_ff, &(_parameters.y_ff));
	param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max));
	param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax));
	param_get(_parameter_handles.roll_to_yaw_ff, &(_parameters.roll_to_yaw_ff));

	int32_t wheel_enabled = 0;
	param_get(_parameter_handles.w_en, &wheel_enabled);
	_parameters.w_en = (wheel_enabled == 1);

	param_get(_parameter_handles.w_p, &(_parameters.w_p));
	param_get(_parameter_handles.w_i, &(_parameters.w_i));
	param_get(_parameter_handles.w_ff, &(_parameters.w_ff));
	param_get(_parameter_handles.w_integrator_max, &(_parameters.w_integrator_max));
	param_get(_parameter_handles.w_rmax, &(_parameters.w_rmax));

	param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min));
	param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim));
	param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max));

	param_get(_parameter_handles.trim_roll, &(_parameters.trim_roll));
	param_get(_parameter_handles.trim_pitch, &(_parameters.trim_pitch));
	param_get(_parameter_handles.trim_yaw, &(_parameters.trim_yaw));
	param_get(_parameter_handles.rollsp_offset_deg, &(_parameters.rollsp_offset_deg));
	param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg));
	_parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg);
	_parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg);
	param_get(_parameter_handles.man_roll_max, &(_parameters.man_roll_max));
	param_get(_parameter_handles.man_pitch_max, &(_parameters.man_pitch_max));
	_parameters.man_roll_max = math::radians(_parameters.man_roll_max);
	_parameters.man_pitch_max = math::radians(_parameters.man_pitch_max);
	param_get(_parameter_handles.man_roll_scale, &(_parameters.man_roll_scale));
	param_get(_parameter_handles.man_pitch_scale, &(_parameters.man_pitch_scale));
	param_get(_parameter_handles.man_yaw_scale, &(_parameters.man_yaw_scale));

	param_get(_parameter_handles.acro_max_x_rate, &(_parameters.acro_max_x_rate_rad));
	param_get(_parameter_handles.acro_max_y_rate, &(_parameters.acro_max_y_rate_rad));
	param_get(_parameter_handles.acro_max_z_rate, &(_parameters.acro_max_z_rate_rad));
	_parameters.acro_max_x_rate_rad = math::radians(_parameters.acro_max_x_rate_rad);
	_parameters.acro_max_y_rate_rad = math::radians(_parameters.acro_max_y_rate_rad);
	_parameters.acro_max_z_rate_rad = math::radians(_parameters.acro_max_z_rate_rad);

	param_get(_parameter_handles.flaps_scale, &_parameters.flaps_scale);
	param_get(_parameter_handles.flaperon_scale, &_parameters.flaperon_scale);

	param_get(_parameter_handles.rattitude_thres, &_parameters.rattitude_thres);

	if (_vehicle_status.is_vtol) {
		param_get(_parameter_handles.vtol_type, &_parameters.vtol_type);
	}

	param_get(_parameter_handles.bat_scale_en, &_parameters.bat_scale_en);

	/* pitch control parameters */
	_pitch_ctrl.set_time_constant(_parameters.p_tc);
	_pitch_ctrl.set_k_p(_parameters.p_p);
	_pitch_ctrl.set_k_i(_parameters.p_i);
	_pitch_ctrl.set_k_ff(_parameters.p_ff);
	_pitch_ctrl.set_integrator_max(_parameters.p_integrator_max);
	_pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos));
	_pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg));

	/* roll control parameters */
	_roll_ctrl.set_time_constant(_parameters.r_tc);
	_roll_ctrl.set_k_p(_parameters.r_p);
	_roll_ctrl.set_k_i(_parameters.r_i);
	_roll_ctrl.set_k_ff(_parameters.r_ff);
	_roll_ctrl.set_integrator_max(_parameters.r_integrator_max);
	_roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax));

	/* yaw control parameters */
	_yaw_ctrl.set_k_p(_parameters.y_p);
	_yaw_ctrl.set_k_i(_parameters.y_i);
	_yaw_ctrl.set_k_ff(_parameters.y_ff);
	_yaw_ctrl.set_integrator_max(_parameters.y_integrator_max);
	_yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax));

	/* wheel control parameters */
	_wheel_ctrl.set_k_p(_parameters.w_p);
//.........这里部分代码省略.........

开发者ID:larics，项目名称:Firmware，代码行数:101，代码来源:fw_att_control_main.cpp

示例3: warnx

void
FixedwingAttitudeControl::task_main()
{

    /* inform about start */
    warnx("Initializing..");
    fflush(stdout);

    /*
     * do subscriptions
     */
    _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
    _att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
    _accel_sub = orb_subscribe(ORB_ID(sensor_accel));
    _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));

    /* rate limit vehicle status updates to 5Hz */
    orb_set_interval(_vcontrol_mode_sub, 200);
    /* rate limit attitude control to 50 Hz (with some margin, so 17 ms) */
    orb_set_interval(_att_sub, 17);

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

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

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

            vehicle_airspeed_poll();

            vehicle_setpoint_poll();

            vehicle_accel_poll();

            vehicle_control_mode_poll();

            vehicle_manual_poll();

            global_pos_poll();

            /* lock integrator until control is started */
//.........这里部分代码省略.........

开发者ID:KjeldJensen，项目名称:Firmware，代码行数:101，代码来源:fw_att_control_main.cpp

示例4:

int
FixedwingAttitudeControl::parameters_update()
{

    param_get(_parameter_handles.tconst, &(_parameters.tconst));
    param_get(_parameter_handles.p_p, &(_parameters.p_p));
    param_get(_parameter_handles.p_i, &(_parameters.p_i));
    param_get(_parameter_handles.p_ff, &(_parameters.p_ff));
    param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos));
    param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg));
    param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max));
    param_get(_parameter_handles.p_roll_feedforward, &(_parameters.p_roll_feedforward));

    param_get(_parameter_handles.r_p, &(_parameters.r_p));
    param_get(_parameter_handles.r_i, &(_parameters.r_i));
    param_get(_parameter_handles.r_ff, &(_parameters.r_ff));

    param_get(_parameter_handles.r_integrator_max, &(_parameters.r_integrator_max));
    param_get(_parameter_handles.r_rmax, &(_parameters.r_rmax));

    param_get(_parameter_handles.y_p, &(_parameters.y_p));
    param_get(_parameter_handles.y_i, &(_parameters.y_i));
    param_get(_parameter_handles.y_ff, &(_parameters.y_ff));
    param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max));
    param_get(_parameter_handles.y_coordinated_min_speed, &(_parameters.y_coordinated_min_speed));
    param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax));

    param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min));
    param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim));
    param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max));

    param_get(_parameter_handles.trim_roll, &(_parameters.trim_roll));
    param_get(_parameter_handles.trim_pitch, &(_parameters.trim_pitch));
    param_get(_parameter_handles.trim_yaw, &(_parameters.trim_yaw));
    param_get(_parameter_handles.rollsp_offset_deg, &(_parameters.rollsp_offset_deg));
    param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg));
    _parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg);
    _parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg);
    param_get(_parameter_handles.man_roll_max, &(_parameters.man_roll_max));
    param_get(_parameter_handles.man_pitch_max, &(_parameters.man_pitch_max));
    _parameters.man_roll_max = math::radians(_parameters.man_roll_max);
    _parameters.man_pitch_max = math::radians(_parameters.man_pitch_max);


    /* pitch control parameters */
    _pitch_ctrl.set_time_constant(_parameters.tconst);
    _pitch_ctrl.set_k_p(_parameters.p_p);
    _pitch_ctrl.set_k_i(_parameters.p_i);
    _pitch_ctrl.set_k_ff(_parameters.p_ff);
    _pitch_ctrl.set_integrator_max(_parameters.p_integrator_max);
    _pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos));
    _pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg));
    _pitch_ctrl.set_roll_ff(_parameters.p_roll_feedforward);

    /* roll control parameters */
    _roll_ctrl.set_time_constant(_parameters.tconst);
    _roll_ctrl.set_k_p(_parameters.r_p);
    _roll_ctrl.set_k_i(_parameters.r_i);
    _roll_ctrl.set_k_ff(_parameters.r_ff);
    _roll_ctrl.set_integrator_max(_parameters.r_integrator_max);
    _roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax));

    /* yaw control parameters */
    _yaw_ctrl.set_k_p(_parameters.y_p);
    _yaw_ctrl.set_k_i(_parameters.y_i);
    _yaw_ctrl.set_k_ff(_parameters.y_ff);
    _yaw_ctrl.set_integrator_max(_parameters.y_integrator_max);
    _yaw_ctrl.set_coordinated_min_speed(_parameters.y_coordinated_min_speed);
    _yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax));

    return OK;
}

开发者ID:KjeldJensen，项目名称:Firmware，代码行数:72，代码来源:fw_att_control_main.cpp

示例5:

int
FixedwingAttitudeControl::parameters_update()
{

	param_get(_parameter_handles.p_tc, &(_parameters.p_tc));
	param_get(_parameter_handles.p_p, &(_parameters.p_p));
	param_get(_parameter_handles.p_i, &(_parameters.p_i));
	param_get(_parameter_handles.p_ff, &(_parameters.p_ff));
	param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos));
	param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg));
	param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max));

	param_get(_parameter_handles.r_tc, &(_parameters.r_tc));
	param_get(_parameter_handles.r_p, &(_parameters.r_p));
	param_get(_parameter_handles.r_i, &(_parameters.r_i));
	param_get(_parameter_handles.r_ff, &(_parameters.r_ff));

	param_get(_parameter_handles.r_integrator_max, &(_parameters.r_integrator_max));
	param_get(_parameter_handles.r_rmax, &(_parameters.r_rmax));

	param_get(_parameter_handles.y_p, &(_parameters.y_p));
	param_get(_parameter_handles.y_i, &(_parameters.y_i));
	param_get(_parameter_handles.y_ff, &(_parameters.y_ff));
	param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max));
	param_get(_parameter_handles.y_coordinated_min_speed, &(_parameters.y_coordinated_min_speed));
	param_get(_parameter_handles.y_coordinated_method, &(_parameters.y_coordinated_method));
	param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax));

	param_get(_parameter_handles.w_p, &(_parameters.w_p));
	param_get(_parameter_handles.w_i, &(_parameters.w_i));
	param_get(_parameter_handles.w_ff, &(_parameters.w_ff));
	param_get(_parameter_handles.w_integrator_max, &(_parameters.w_integrator_max));
	param_get(_parameter_handles.w_rmax, &(_parameters.w_rmax));

	param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min));
	param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim));
	param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max));

	param_get(_parameter_handles.trim_roll, &(_parameters.trim_roll));
	param_get(_parameter_handles.trim_pitch, &(_parameters.trim_pitch));
	param_get(_parameter_handles.trim_yaw, &(_parameters.trim_yaw));
	param_get(_parameter_handles.trim_steer, &(_parameters.trim_steer));
	param_get(_parameter_handles.rollsp_offset_deg, &(_parameters.rollsp_offset_deg));
	param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg));
	_parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg);
	_parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg);
	param_get(_parameter_handles.man_roll_max, &(_parameters.man_roll_max));
	param_get(_parameter_handles.man_pitch_max, &(_parameters.man_pitch_max));
	_parameters.man_roll_max = math::radians(_parameters.man_roll_max);
	_parameters.man_pitch_max = math::radians(_parameters.man_pitch_max);
	param_get(_parameter_handles.man_roll_scale, &(_parameters.man_roll_scale));
	param_get(_parameter_handles.man_pitch_scale, &(_parameters.man_pitch_scale));
	param_get(_parameter_handles.man_yaw_scale, &(_parameters.man_yaw_scale));

	param_get(_parameter_handles.flaps_scale, &_parameters.flaps_scale);
	param_get(_parameter_handles.flaperon_scale, &_parameters.flaperon_scale);

	param_get(_parameter_handles.vtol_type, &_parameters.vtol_type);

	/* pitch control parameters */
	_pitch_ctrl.set_time_constant(_parameters.p_tc);
	_pitch_ctrl.set_k_p(_parameters.p_p);
	_pitch_ctrl.set_k_i(_parameters.p_i);
	_pitch_ctrl.set_k_ff(_parameters.p_ff);
	_pitch_ctrl.set_integrator_max(_parameters.p_integrator_max);
	_pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos));
	_pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg));

	/* roll control parameters */
	_roll_ctrl.set_time_constant(_parameters.r_tc);
	_roll_ctrl.set_k_p(_parameters.r_p);
	_roll_ctrl.set_k_i(_parameters.r_i);
	_roll_ctrl.set_k_ff(_parameters.r_ff);
	_roll_ctrl.set_integrator_max(_parameters.r_integrator_max);
	_roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax));

	/* yaw control parameters */
	_yaw_ctrl.set_k_p(_parameters.y_p);
	_yaw_ctrl.set_k_i(_parameters.y_i);
	_yaw_ctrl.set_k_ff(_parameters.y_ff);
	_yaw_ctrl.set_integrator_max(_parameters.y_integrator_max);
	_yaw_ctrl.set_coordinated_min_speed(_parameters.y_coordinated_min_speed);
	_yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method);
	_yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax));

	/* wheel control parameters */
	_wheel_ctrl.set_k_p(_parameters.w_p);
	_wheel_ctrl.set_k_i(_parameters.w_i);
	_wheel_ctrl.set_k_ff(_parameters.w_ff);
	_wheel_ctrl.set_integrator_max(_parameters.w_integrator_max);
	_wheel_ctrl.set_max_rate(math::radians(_parameters.w_rmax));

	return PX4_OK;
}

开发者ID:KamalGalrani，项目名称:Firmware，代码行数:94，代码来源:fw_att_control_main.cpp

示例6: poll

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

开发者ID:harvard-uas，项目名称:Firmware，代码行数:101，代码来源:fw_att_control_main.cpp

注：本文中的ECL_RollController类示例由纯净天空整理自Github/MSDocs等开源代码及文档管理平台，相关代码片段筛选自各路编程大神贡献的开源项目，源码版权归原作者所有，传播和使用请参考对应项目的License；未经允许，请勿转载。