int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
		const float rates[],
		struct actuator_controls_s *actuators)
{
	static int counter = 0;
	static bool initialized = false;

	static struct fw_rate_control_params p;
	static struct fw_rate_control_param_handles h;

	static PID_t roll_rate_controller;
	static PID_t pitch_rate_controller;
	static PID_t yaw_rate_controller;

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

	if(!initialized)
	{
		parameters_init(&h);
		parameters_update(&h, &p);
		pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the controller layout is with a PI rate controller
		pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
		pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
		initialized = true;
	}

	/* load new parameters with lower rate */
	if (counter % 100 == 0) {
		/* update parameters from storage */
		parameters_update(&h, &p);
		pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1);
		pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1);
		pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1);
	}


	/* Roll Rate (PI) */
	actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0, deltaT);


	actuators->control[1] = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0, deltaT); //TODO: (-) sign comes from the elevator (positive --> deflection downwards), this has to be moved to the mixer...
	actuators->control[2] = 0;//pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0, deltaT); 	//XXX TODO disabled for now

	counter++;

	return 0;
}

void parameters_init()
{
	_params_handles.accel_x_offset		=	param_find("CAL_ACC0_XOFF");
	_params_handles.accel_x_scale		=	param_find("CAL_ACC0_XSCALE");
	_params_handles.accel_y_offset		=	param_find("CAL_ACC0_YOFF");
	_params_handles.accel_y_scale		=	param_find("CAL_ACC0_YSCALE");
	_params_handles.accel_z_offset		=	param_find("CAL_ACC0_ZOFF");
	_params_handles.accel_z_scale		=	param_find("CAL_ACC0_ZSCALE");

	_params_handles.gyro_x_offset		=	param_find("CAL_GYRO0_XOFF");
	_params_handles.gyro_x_scale		=	param_find("CAL_GYRO0_XSCALE");
	_params_handles.gyro_y_offset		=	param_find("CAL_GYRO0_YOFF");
	_params_handles.gyro_y_scale		=	param_find("CAL_GYRO0_YSCALE");
	_params_handles.gyro_z_offset		=	param_find("CAL_GYRO0_ZOFF");
	_params_handles.gyro_z_scale		=	param_find("CAL_GYRO0_ZSCALE");

	_params_handles.mag_x_offset		=	param_find("CAL_MAG0_XOFF");
	_params_handles.mag_x_scale		=	param_find("CAL_MAG0_XSCALE");
	_params_handles.mag_y_offset		=	param_find("CAL_MAG0_YOFF");
	_params_handles.mag_y_scale		=	param_find("CAL_MAG0_YSCALE");
	_params_handles.mag_z_offset		=	param_find("CAL_MAG0_ZOFF");
	_params_handles.mag_z_scale		=	param_find("CAL_MAG0_ZSCALE");

	_params_handles.gyro_lpf_enum		=	param_find("MPU_GYRO_LPF_ENM");
	_params_handles.accel_lpf_enum		=	param_find("MPU_ACC_LPF_ENM");

	_params_handles.imu_sample_rate_enum	=	param_find("MPU_SAMPLE_R_ENM");

	parameters_update();
}

estimator_base::estimator_base()
{
    _time_to_run = -1;

    _params_sub = orb_subscribe(ORB_ID(parameter_update));
    _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
//    _airspeed_sub = orb_subscribe(ORB_ID(airspeed));
    _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
    _gps_init = false;
    _baro_init = false;
    fds[0].fd = _sensor_combined_sub;
    fds[0].events = POLLIN;
    poll_error_counter = 0;

    memset(&_params, 0, sizeof(_params));
    memset(&_sensor_combined, 0, sizeof(_sensor_combined));
    memset(&_gps, 0, sizeof(_gps));
    memset(&_vehicle_state, 0, sizeof(_vehicle_state));

    _params_handles.gravity        = param_find("UAVBOOK_GRAVITY");
    _params_handles.rho            = param_find("UAVBOOK_RHO");
    _params_handles.sigma_accel    = param_find("UAVBOOK_SIGMA_ACCEL");
    _params_handles.sigma_n_gps    = param_find("UAVBOOK_SIGMA_N_GPS");
    _params_handles.sigma_e_gps    = param_find("UAVBOOK_SIGMA_E_GPS");
    _params_handles.sigma_Vg_gps   = param_find("UAVBOOK_SIGMA_VG_GPS");
    _params_handles.sigma_course_gps       = param_find("UAVBOOK_SIGMA_COURSE_GPS");

    parameters_update();

    _vehicle_state_pub = orb_advertise(ORB_ID(vehicle_state), &_vehicle_state);
}

int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
		const struct vehicle_attitude_s *att,
		struct vehicle_rates_setpoint_s *rates_sp)
{
	static int counter = 0;
	static bool initialized = false;

	static struct fw_att_control_params p;
	static struct fw_pos_control_param_handles h;

	static PID_t roll_controller;
	static PID_t pitch_controller;


	if(!initialized)
	{
		parameters_init(&h);
		parameters_update(&h, &p);
		pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller
		pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller
		initialized = true;
	}

	/* load new parameters with lower rate */
	if (counter % 100 == 0) {
		/* update parameters from storage */
		parameters_update(&h, &p);
		pid_set_parameters(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim);
		pid_set_parameters(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim);
	}

	/* Roll (P) */
	rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_tait_bryan, att->roll, 0, 0);

	/* Pitch (P) */
	float pitch_sp_rollcompensation = att_sp->pitch_tait_bryan + p.pitch_roll_compensation_p * att_sp->roll_tait_bryan;
	rates_sp->pitch = pid_calculate(&pitch_controller, pitch_sp_rollcompensation, att->pitch, 0, 0);

	/* Yaw (from coordinated turn constraint or lateral force) */
	//TODO

	counter++;

	return 0;
}

void path_follower_base::parameter_update_poll()
{
  bool updated;

  /* Check if param status has changed */
  orb_check(_params_sub, &updated);

  if (updated) {
    struct parameter_update_s param_update;
    orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);
    parameters_update();
  }
}

void
MulticopterAttitudeControl::parameter_update_poll()
{
	bool updated;

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

	if (updated) {
		struct parameter_update_s param_update;
		orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);
		parameters_update();
	}
}

/**
* Check for parameter updates.
*/
void
VtolAttitudeControl::parameters_update_poll()
{
	bool updated;

	/* Check if parameters have changed */
	orb_check(_params_sub, &updated);

	if (updated) {
		struct parameter_update_s param_update;
		orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);
		parameters_update();
	}
}

GroundRoverAttitudeControl::GroundRoverAttitudeControl() :
	/* performance counters */
	_loop_perf(perf_alloc(PC_ELAPSED, "gnda_dt")),

	_nonfinite_input_perf(perf_alloc(PC_COUNT, "gnda_nani")),
	_nonfinite_output_perf(perf_alloc(PC_COUNT, "gnda_nano"))
{
	_parameter_handles.w_p = param_find("GND_WR_P");
	_parameter_handles.w_i = param_find("GND_WR_I");
	_parameter_handles.w_d = param_find("GND_WR_D");
	_parameter_handles.w_imax = param_find("GND_WR_IMAX");

	_parameter_handles.trim_yaw = param_find("TRIM_YAW");

	_parameter_handles.man_yaw_scale = param_find("GND_MAN_Y_SC");

	_parameter_handles.bat_scale_en = param_find("GND_BAT_SCALE_EN");

	/* fetch initial parameter values */
	parameters_update();
}

/**
* Constructor
*/
VtolAttitudeControl::VtolAttitudeControl()
{
	_vtol_vehicle_status.vtol_in_rw_mode = true;	/* start vtol in rotary wing mode*/

	_params.idle_pwm_mc = PWM_DEFAULT_MIN;
	_params.vtol_motor_count = 0;

	_params_handles.idle_pwm_mc = param_find("VT_IDLE_PWM_MC");
	_params_handles.vtol_motor_count = param_find("VT_MOT_COUNT");
	_params_handles.vtol_fw_permanent_stab = param_find("VT_FW_PERM_STAB");
	_params_handles.fw_pitch_trim = param_find("VT_FW_PITCH_TRIM");
	_params_handles.vtol_type = param_find("VT_TYPE");
	_params_handles.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK");
	_params_handles.fw_min_alt = param_find("VT_FW_MIN_ALT");
	_params_handles.fw_alt_err = param_find("VT_FW_ALT_ERR");
	_params_handles.fw_qc_max_pitch = param_find("VT_FW_QC_P");
	_params_handles.fw_qc_max_roll = param_find("VT_FW_QC_R");
	_params_handles.front_trans_time_openloop = param_find("VT_F_TR_OL_TM");
	_params_handles.front_trans_time_min = param_find("VT_TRANS_MIN_TM");

	_params_handles.wv_takeoff = param_find("VT_WV_TKO_EN");
	_params_handles.wv_land = param_find("VT_WV_LND_EN");
	_params_handles.wv_loiter = param_find("VT_WV_LTR_EN");

	/* fetch initial parameter values */
	parameters_update();

	if (_params.vtol_type == vtol_type::TAILSITTER) {
		_vtol_type = new Tailsitter(this);

	} else if (_params.vtol_type == vtol_type::TILTROTOR) {
		_vtol_type = new Tiltrotor(this);

	} else if (_params.vtol_type == vtol_type::STANDARD) {
		_vtol_type = new Standard(this);

	} else {
		_task_should_exit = true;
	}
}

path_follower_base::path_follower_base()
{
    _params_sub = orb_subscribe(ORB_ID(parameter_update));
    _vehicle_state_sub = orb_subscribe(ORB_ID(vehicle_state));
    _current_path_sub = orb_subscribe(ORB_ID(current_path));
    fds[0].fd = _vehicle_state_sub;
    fds[0].events = POLLIN;
    poll_error_counter = 0;

    memset(&_vehicle_state, 0, sizeof(_vehicle_state));
    memset(&_current_path, 0, sizeof(_current_path));
    memset(&_controller_commands, 0, sizeof(_controller_commands));
    memset(&_params, 0, sizeof(_params));

    _params_handles.chi_infty      = param_find("UAVBOOK_CHI_INFTY");
    _params_handles.k_path         = param_find("UAVBOOK_K_PATH");
    _params_handles.k_orbit        = param_find("UAVBOOK_K_ORBIT");

    parameters_update();

    _controller_commands_pub = orb_advertise(ORB_ID(controller_commands), &_controller_commands);
}

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

MulticopterAttitudeControl::MulticopterAttitudeControl() :

	_task_should_exit(false),
	_control_task(-1),

/* subscriptions */
	_v_att_sub(-1),
	_v_att_sp_sub(-1),
	_v_control_mode_sub(-1),
	_params_sub(-1),
	_manual_control_sp_sub(-1),
	_armed_sub(-1),

/* publications */
	_att_sp_pub(-1),
	_v_rates_sp_pub(-1),
	_actuators_0_pub(-1),

/* performance counters */
	_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))

{
	memset(&_v_att, 0, sizeof(_v_att));
	memset(&_v_att_sp, 0, sizeof(_v_att_sp));
	memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
	memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
	memset(&_v_control_mode, 0, sizeof(_v_control_mode));
	memset(&_actuators, 0, sizeof(_actuators));
	memset(&_armed, 0, sizeof(_armed));

	_params.att_p.zero();
	_params.rate_p.zero();
	_params.rate_i.zero();
	_params.rate_d.zero();
	_params.yaw_ff = 0.0f;
	_params.yaw_rate_max = 0.0f;
	_params.man_roll_max = 0.0f;
	_params.man_pitch_max = 0.0f;
	_params.man_yaw_max = 0.0f;

	_rates_prev.zero();
	_rates_sp.zero();
	_rates_int.zero();
	_thrust_sp = 0.0f;
	_att_control.zero();

	_I.identity();

	_params_handles.roll_p			= 	param_find("MC_ROLL_P");
	_params_handles.roll_rate_p		= 	param_find("MC_ROLLRATE_P");
	_params_handles.roll_rate_i		= 	param_find("MC_ROLLRATE_I");
	_params_handles.roll_rate_d		= 	param_find("MC_ROLLRATE_D");
	_params_handles.pitch_p			= 	param_find("MC_PITCH_P");
	_params_handles.pitch_rate_p	= 	param_find("MC_PITCHRATE_P");
	_params_handles.pitch_rate_i	= 	param_find("MC_PITCHRATE_I");
	_params_handles.pitch_rate_d	= 	param_find("MC_PITCHRATE_D");
	_params_handles.yaw_p			=	param_find("MC_YAW_P");
	_params_handles.yaw_rate_p		= 	param_find("MC_YAWRATE_P");
	_params_handles.yaw_rate_i		= 	param_find("MC_YAWRATE_I");
	_params_handles.yaw_rate_d		= 	param_find("MC_YAWRATE_D");
	_params_handles.yaw_ff			= 	param_find("MC_YAW_FF");
	_params_handles.yaw_rate_max	= 	param_find("MC_YAWRATE_MAX");
	_params_handles.man_roll_max	= 	param_find("MC_MAN_R_MAX");
	_params_handles.man_pitch_max	= 	param_find("MC_MAN_P_MAX");
	_params_handles.man_yaw_max		= 	param_find("MC_MAN_Y_MAX");

	/* fetch initial parameter values */
	parameters_update();
}

//.........这里部分代码省略.........
    while (!thread_should_exit) {

        struct pollfd fds[2];
        fds[0].fd = sub_raw;
        fds[0].events = POLLIN;
        fds[1].fd = sub_params;
        fds[1].events = POLLIN;
        int ret = poll(fds, 2, 1000);

        /* Added by Ross Allen */
        //~ 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];

void Tiltrotor::update_vtol_state()
{
	parameters_update();

	/* simple logic using a two way switch to perform transitions.
	 * after flipping the switch the vehicle will start tilting rotors, picking up
	 * forward speed. After the vehicle has picked up enough speed the rotors are tilted
	 * forward completely. For the backtransition the motors simply rotate back.
	*/

	if (!_attc->is_fixed_wing_requested()) {

		// plane is in multicopter mode
		switch (_vtol_schedule.flight_mode) {
		case MC_MODE:
			break;

		case FW_MODE:
			_vtol_schedule.flight_mode 	= TRANSITION_BACK;
			_vtol_schedule.transition_start = hrt_absolute_time();
			break;

		case TRANSITION_FRONT_P1:
			// failsafe into multicopter mode
			_vtol_schedule.flight_mode = MC_MODE;
			break;

		case TRANSITION_FRONT_P2:
			// failsafe into multicopter mode
			_vtol_schedule.flight_mode = MC_MODE;
			break;

		case TRANSITION_BACK:
			if (_tilt_control <= _params_tiltrotor.tilt_mc) {
				_vtol_schedule.flight_mode = MC_MODE;
			}

			break;
		}

	} else {

		switch (_vtol_schedule.flight_mode) {
		case MC_MODE:
			// initialise a front transition
			_vtol_schedule.flight_mode 	= TRANSITION_FRONT_P1;
			_vtol_schedule.transition_start = hrt_absolute_time();
			break;

		case FW_MODE:
			break;

		case TRANSITION_FRONT_P1:

			// check if we have reached airspeed to switch to fw mode
			if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_trans) {
				_vtol_schedule.flight_mode = TRANSITION_FRONT_P2;
				_vtol_schedule.transition_start = hrt_absolute_time();
			}

			break;

		case TRANSITION_FRONT_P2:

			// if the rotors have been tilted completely we switch to fw mode
			if (_tilt_control >= _params_tiltrotor.tilt_fw) {
				_vtol_schedule.flight_mode = FW_MODE;
				_tilt_control = _params_tiltrotor.tilt_fw;
			}

			break;

		case TRANSITION_BACK:
			// failsafe into fixed wing mode
			_vtol_schedule.flight_mode = FW_MODE;
			break;
		}
	}

	// map tiltrotor specific control phases to simple control modes
	switch (_vtol_schedule.flight_mode) {
	case MC_MODE:
		_vtol_mode = ROTARY_WING;
		break;

	case FW_MODE:
		_vtol_mode = FIXED_WING;
		break;

	case TRANSITION_FRONT_P1:
	case TRANSITION_FRONT_P2:
	case TRANSITION_BACK:
		_vtol_mode = TRANSITION;
		break;
	}
}

//.........这里部分代码省略.........
	_battery_current_timestamp(0)
{
	memset(&_rc, 0, sizeof(_rc));

	/* basic r/c parameters */
	for (unsigned i = 0; i < _rc_max_chan_count; i++) {
		char nbuf[16];

		/* min values */
		sprintf(nbuf, "RC%d_MIN", i + 1);
		_parameter_handles.min[i] = param_find(nbuf);

		/* trim values */
		sprintf(nbuf, "RC%d_TRIM", i + 1);
		_parameter_handles.trim[i] = param_find(nbuf);

		/* max values */
		sprintf(nbuf, "RC%d_MAX", i + 1);
		_parameter_handles.max[i] = param_find(nbuf);

		/* channel reverse */
		sprintf(nbuf, "RC%d_REV", i + 1);
		_parameter_handles.rev[i] = param_find(nbuf);

		/* channel deadzone */
		sprintf(nbuf, "RC%d_DZ", i + 1);
		_parameter_handles.dz[i] = param_find(nbuf);

	}

	/* mandatory input switched, mapped to channels 1-4 per default */
	_parameter_handles.rc_map_roll 	= param_find("RC_MAP_ROLL");
	_parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH");
	_parameter_handles.rc_map_yaw 	= param_find("RC_MAP_YAW");
	_parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE");
	_parameter_handles.rc_map_failsafe = param_find("RC_MAP_FAILSAFE");

	/* mandatory mode switches, mapped to channel 5 and 6 per default */
	_parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW");
	_parameter_handles.rc_map_return_sw = param_find("RC_MAP_RETURN_SW");

	_parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS");

	/* optional mode switches, not mapped per default */
	_parameter_handles.rc_map_posctl_sw = param_find("RC_MAP_POSCTL_SW");
	_parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW");

	_parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1");
	_parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2");
	_parameter_handles.rc_map_aux3 = param_find("RC_MAP_AUX3");
	_parameter_handles.rc_map_aux4 = param_find("RC_MAP_AUX4");
	_parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5");

	/* RC thresholds */
	_parameter_handles.rc_fails_thr = param_find("RC_FAILS_THR");
	_parameter_handles.rc_assist_th = param_find("RC_ASSIST_TH");
	_parameter_handles.rc_auto_th = param_find("RC_AUTO_TH");
	_parameter_handles.rc_posctl_th = param_find("RC_POSCTL_TH");
	_parameter_handles.rc_return_th = param_find("RC_RETURN_TH");
	_parameter_handles.rc_loiter_th = param_find("RC_LOITER_TH");

	/* gyro offsets */
	_parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF");
	_parameter_handles.gyro_offset[1] = param_find("SENS_GYRO_YOFF");
	_parameter_handles.gyro_offset[2] = param_find("SENS_GYRO_ZOFF");
	_parameter_handles.gyro_scale[0] = param_find("SENS_GYRO_XSCALE");
	_parameter_handles.gyro_scale[1] = param_find("SENS_GYRO_YSCALE");
	_parameter_handles.gyro_scale[2] = param_find("SENS_GYRO_ZSCALE");

	/* accel offsets */
	_parameter_handles.accel_offset[0] = param_find("SENS_ACC_XOFF");
	_parameter_handles.accel_offset[1] = param_find("SENS_ACC_YOFF");
	_parameter_handles.accel_offset[2] = param_find("SENS_ACC_ZOFF");
	_parameter_handles.accel_scale[0] = param_find("SENS_ACC_XSCALE");
	_parameter_handles.accel_scale[1] = param_find("SENS_ACC_YSCALE");
	_parameter_handles.accel_scale[2] = param_find("SENS_ACC_ZSCALE");

	/* mag offsets */
	_parameter_handles.mag_offset[0] = param_find("SENS_MAG_XOFF");
	_parameter_handles.mag_offset[1] = param_find("SENS_MAG_YOFF");
	_parameter_handles.mag_offset[2] = param_find("SENS_MAG_ZOFF");

	_parameter_handles.mag_scale[0] = param_find("SENS_MAG_XSCALE");
	_parameter_handles.mag_scale[1] = param_find("SENS_MAG_YSCALE");
	_parameter_handles.mag_scale[2] = param_find("SENS_MAG_ZSCALE");

	/* Differential pressure offset */
	_parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
	_parameter_handles.diff_pres_analog_enabled = param_find("SENS_DPRES_ANA");

	_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
	_parameter_handles.battery_current_scaling = param_find("BAT_C_SCALING");

	/* rotations */
	_parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
	_parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT");

	/* fetch initial parameter values */
	parameters_update();
}

//.........这里部分代码省略.........
	memset(&ref, 0, sizeof(ref));
	hrt_abstime home_timestamp = 0;

	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;

	/* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */
	float 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_vision[3][2] = {
		{ 0.0f, 0.0f },		// N (pos, vel)
		{ 0.0f, 0.0f },		// E (pos, vel)
		{ 0.0f, 0.0f },		// D (pos, vel)
	};
	
	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 flow_prev = 0;			// time of last flow measurement
	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)

	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)
	bool vision_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 home_position_s home;
	memset(&home, 0, sizeof(home));
	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 vision_position_estimate vision;
	memset(&vision, 0, sizeof(vision));
	struct vehicle_global_position_s global_pos;
	memset(&global_pos, 0, sizeof(global_pos));

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

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

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

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

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

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

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

	/* initialize parameters cache */
	parameters_update();

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

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

	while (!_task_should_exit) {

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

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

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

		perf_begin(_loop_perf);

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

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

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

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

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

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

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

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

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

			} else {
				/* attitude controller disabled, poll rates setpoint topic */
				vehicle_rates_setpoint_poll();
				_rates_sp(0) = _v_rates_sp.roll;
				_rates_sp(1) = _v_rates_sp.pitch;
				_rates_sp(2) = _v_rates_sp.yaw;
				_thrust_sp = _v_rates_sp.thrust;
			}

			if (_v_control_mode.flag_control_rates_enabled) {
				control_attitude_rates(dt);

				/* publish actuator controls */
				_actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f;
				_actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
//.........这里部分代码省略.........

FixedwingAttitudeControl::FixedwingAttitudeControl() :

    _task_should_exit(false),
    _control_task(-1),

    /* subscriptions */
    _att_sub(-1),
    _accel_sub(-1),
    _airspeed_sub(-1),
    _vcontrol_mode_sub(-1),
    _params_sub(-1),
    _manual_sub(-1),
    _global_pos_sub(-1),

    /* publications */
    _rate_sp_pub(-1),
    _attitude_sp_pub(-1),
    _actuators_0_pub(-1),
    _actuators_1_pub(-1),

    /* performance counters */
    _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")),
    _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")),
    _nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")),
    /* states */
    _setpoint_valid(false)
{
    /* safely initialize structs */
    _att = {};
    _accel = {};
    _att_sp = {};
    _manual = {};
    _airspeed = {};
    _vcontrol_mode = {};
    _actuators = {};
    _actuators_airframe = {};
    _global_pos = {};


    _parameter_handles.tconst = param_find("FW_ATT_TC");
    _parameter_handles.p_p = param_find("FW_PR_P");
    _parameter_handles.p_i = param_find("FW_PR_I");
    _parameter_handles.p_ff = param_find("FW_PR_FF");
    _parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS");
    _parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG");
    _parameter_handles.p_integrator_max = param_find("FW_PR_IMAX");
    _parameter_handles.p_roll_feedforward = param_find("FW_P_ROLLFF");

    _parameter_handles.r_p = param_find("FW_RR_P");
    _parameter_handles.r_i = param_find("FW_RR_I");
    _parameter_handles.r_ff = param_find("FW_RR_FF");
    _parameter_handles.r_integrator_max = param_find("FW_RR_IMAX");
    _parameter_handles.r_rmax = param_find("FW_R_RMAX");

    _parameter_handles.y_p = param_find("FW_YR_P");
    _parameter_handles.y_i = param_find("FW_YR_I");
    _parameter_handles.y_ff = param_find("FW_YR_FF");
    _parameter_handles.y_integrator_max = param_find("FW_YR_IMAX");
    _parameter_handles.y_rmax = param_find("FW_Y_RMAX");

    _parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN");
    _parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM");
    _parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX");

    _parameter_handles.y_coordinated_min_speed = param_find("FW_YCO_VMIN");

    _parameter_handles.trim_roll = param_find("TRIM_ROLL");
    _parameter_handles.trim_pitch = param_find("TRIM_PITCH");
    _parameter_handles.trim_yaw = param_find("TRIM_YAW");
    _parameter_handles.rollsp_offset_deg = param_find("FW_RSP_OFF");
    _parameter_handles.pitchsp_offset_deg = param_find("FW_PSP_OFF");

    _parameter_handles.man_roll_max = param_find("FW_MAN_R_MAX");
    _parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX");

    /* fetch initial parameter values */
    parameters_update();
}

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));
	_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
	_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 */
	px4_pollfd_struct_t fds[1];

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

	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 */
		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 and control state topics */
			orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);

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