//.........这里部分代码省略.........
			PX4_ERR("unable to reset %s", str);
		}
		param_notify_changes();
#endif
	}

	float accel_offs[max_accel_sens][3];
	float accel_T[max_accel_sens][3][3];
	unsigned active_sensors = 0;

	/* measure and calculate offsets & scales */
	if (res == PX4_OK) {
		calibrate_return cal_return = do_accel_calibration_measurements(mavlink_log_pub, accel_offs, accel_T, &active_sensors);
		if (cal_return == calibrate_return_cancelled) {
			// Cancel message already displayed, nothing left to do
			return PX4_ERROR;
		} else if (cal_return == calibrate_return_ok) {
			res = PX4_OK;
		} else {
			res = PX4_ERROR;
		}
	}

	if (res != PX4_OK) {
		if (active_sensors == 0) {
			calibration_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
		}
		return PX4_ERROR;
	}

	/* measurements completed successfully, rotate calibration values */
	param_t board_rotation_h = param_find("SENS_BOARD_ROT");
	int32_t board_rotation_int;
	param_get(board_rotation_h, &(board_rotation_int));
	enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
	math::Matrix<3, 3> board_rotation;
	get_rot_matrix(board_rotation_id, &board_rotation);
	math::Matrix<3, 3> board_rotation_t = board_rotation.transposed();

	bool tc_locked[3] = {false}; // true when the thermal parameter instance has already been adjusted by the calibrator

	for (unsigned uorb_index = 0; uorb_index < active_sensors; uorb_index++) {

		/* handle individual sensors, one by one */
		math::Vector<3> accel_offs_vec(accel_offs[uorb_index]);
		math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec;
		math::Matrix<3, 3> accel_T_mat(accel_T[uorb_index]);
		math::Matrix<3, 3> 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,

static void
hardware_init(void)
{
	__pdata uint16_t	i;

	// Disable the watchdog timer
	PCA0MD	&= ~0x40;

	// Select the internal oscillator, prescale by 1
	FLSCL	 =  0x40;
	OSCICN	 =  0x8F;
	CLKSEL	 =  0x00;

	// Configure the VDD brown out detector
	VDM0CN	 =  0x80;
	for (i = 0; i < 350; i++);	// Wait 100us for initialization
	RSTSRC	 =  0x06;		// enable brown out and missing clock reset sources

#ifdef _BOARD_RFD900A			// Redefine port skips to override bootloader defs
	P0SKIP  =  0xCF;                // P0 UART avail on XBAR
	P1SKIP  =  0xF8;                // P1 SPI1, CEX0 avail on XBAR 
	P2SKIP  =  0x01;                // P2 CEX3 avail on XBAR, rest GPIO
#endif

	// Configure crossbar for UART
	P0MDOUT	 =  0x10;		// UART Tx push-pull
	SFRPAGE	 =  CONFIG_PAGE;
	P0DRV	 =  0x10;		// UART TX
	SFRPAGE	 =  LEGACY_PAGE;
	XBR0	 =  0x01;		// UART enable

	// SPI1
#ifdef _BOARD_RFD900A
	XBR1	|= 0x44;	// enable SPI in 3-wire mode
	P1MDOUT	|= 0xF5;	// SCK1, MOSI1, MISO1 push-pull
	P2MDOUT	|= 0xFF;	// SCK1, MOSI1, MISO1 push-pull
#else
	XBR1	|= 0x40;	// enable SPI in 3-wire mode
	P1MDOUT	|= 0xF5;	// SCK1, MOSI1, MISO1 push-pull
#endif	
	SFRPAGE	 = CONFIG_PAGE;
	P1DRV	|= 0xF5;	// SPI signals use high-current mode, LEDs and PAEN High current drive
	P2DRV	|= 0xFF;	
	SFRPAGE	 = LEGACY_PAGE;
	SPI1CFG	 = 0x40;	// master mode
	SPI1CN	 = 0x00;	// 3 wire master mode
	SPI1CKR	 = 0x00;	// Initialise SPI prescaler to divide-by-2 (12.25MHz, technically out of spec)
	SPI1CN	|= 0x01;	// enable SPI
	NSS1	 = 1;		// set NSS high

	// Clear the radio interrupt state
	IE0	 = 0;

	// initialise timers
	timer_init();

	// UART - set the configured speed
	serial_init(param_get(PARAM_SERIAL_SPEED));

	// set all interrupts to the same priority level
	IP = 0;

	// global interrupt enable
	EA = 1;

	// Turn on the 'radio running' LED and turn off the bootloader LED
	LED_RADIO = LED_ON;
	LED_BOOTLOADER = LED_OFF;

	// ADC system initialise for temp sensor
	AD0EN = 1;	// Enable ADC0
	ADC0CF = 0xF9;  // Set amp0gn=1 (1:1)
	ADC0AC = 0x00;
	ADC0MX = 0x1B;	// Set ADC0MX to temp sensor
	REF0CN = 0x07;	// Define reference and enable temp sensor

#ifdef _BOARD_RFD900A
	// PCA0, CEX0 setup and enable.
	PCA0MD = 0x88;
	PCA0PWM = 0x00;
	PCA0CPH3 = 0x80;
	PCA0CPM3 = 0x42;
	PCA0CN = 0x40;
#endif
	XBR2	 =  0x40;		// Crossbar (GPIO) enable
}

int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
{
	int result = PX4_OK;
	unsigned calibration_counter = 0;
	const unsigned maxcount = 2400;

	/* give directions */
	calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);

	const unsigned calibration_count = (maxcount * 2) / 3;

	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 (PX4_OK == px4_ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
			paramreset_successful = true;

		} else {
			calibration_log_critical(mavlink_log_pub, "[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) {
			calibration_log_critical(mavlink_log_pub, "[cal] No airspeed sensor found");
			goto error_return;
		}

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

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

	while (calibration_counter < calibration_count) {

		if (calibrate_cancel_check(mavlink_log_pub, 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++;

			/* any differential pressure failure a reason to abort */
			if (diff_pres.error_count != 0) {
				calibration_log_critical(mavlink_log_pub, "[cal] Airspeed sensor is reporting errors (%llu)", diff_pres.error_count);
				calibration_log_critical(mavlink_log_pub, "[cal] Check your wiring before trying again");
				feedback_calibration_failed(mavlink_log_pub);
				goto error_return;
			}

			if (calibration_counter % (calibration_count / 20) == 0) {
				calibration_log_info(mavlink_log_pub, 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_log_pub);
			goto error_return;
		}
	}

	diff_pres_offset = diff_pres_offset / calibration_count;

	if (PX4_ISFINITE(diff_pres_offset)) {

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

void MulticopterLandDetector::_update_params()
{
	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.minThrottle, &_params.minThrottle);
	param_get(_paramHandle.hoverThrottle, &_params.hoverThrottle);
	param_get(_paramHandle.throttleRange, &_params.throttleRange);
	param_get(_paramHandle.minManThrottle, &_params.minManThrottle);
	param_get(_paramHandle.freefall_acc_threshold, &_params.freefall_acc_threshold);
	param_get(_paramHandle.freefall_trigger_time, &_params.freefall_trigger_time);
	_freefall_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1e6f * _params.freefall_trigger_time));
	param_get(_paramHandle.manual_stick_down_threshold, &_params.manual_stick_down_threshold);
	param_get(_paramHandle.altitude_max, &_params.altitude_max);
	param_get(_paramHandle.manual_stick_up_position_takeoff_threshold, &_params.manual_stick_up_position_takeoff_threshold);
	param_get(_paramHandle.landSpeed, &_params.landSpeed);
}

int
SF1XX::init()
{
	int ret = PX4_ERROR;
	int hw_model;
	param_get(param_find("SENS_EN_SF1XX"), &hw_model);

	switch (hw_model) {
	case 0:
		DEVICE_LOG("disabled.");
		return ret;

	case 1:  /* SF10/a (25m 32Hz) */
		_min_distance = 0.01f;
		_max_distance = 25.0f;
		_conversion_interval = 31250;
		break;

	case 2:  /* SF10/b (50m 32Hz) */
		_min_distance = 0.01f;
		_max_distance = 50.0f;
		_conversion_interval = 31250;
		break;

	case 3:  /* SF10/c (100m 16Hz) */
		_min_distance = 0.01f;
		_max_distance = 100.0f;
		_conversion_interval = 62500;
		break;

	case 4:
		/* SF11/c (120m 20Hz) */
		_min_distance = 0.01f;
		_max_distance = 120.0f;
		_conversion_interval = 50000;
		break;

	case 5:
		/* SF20/LW20 (100m 48-388Hz) */
		_min_distance = 0.001f;
		_max_distance = 100.0f;
		_conversion_interval = 20834;
		break;

	default:
		DEVICE_LOG("invalid HW model %d.", hw_model);
		return ret;
	}

	/* do I2C init (and probe) first */
	if (I2C::init() != OK) {
		return ret;
	}

	/* allocate basic report buffers */
	_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));

	set_address(SF1XX_BASEADDR);

	if (_reports == nullptr) {
		return ret;
	}

	_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);

	/* get a publish handle on the range finder topic */
	struct distance_sensor_s ds_report = {};

	_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
				 &_orb_class_instance, ORB_PRIO_HIGH);

	if (_distance_sensor_topic == nullptr) {
		DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
	}

	// Select altitude register
	int ret2 = measure();

	if (ret2 == 0) {
		ret = OK;
		_sensor_ok = true;
		DEVICE_LOG("(%dm %dHz) with address %d found", (int)_max_distance,
			   (int)(1e6f / _conversion_interval), SF1XX_BASEADDR);
	}

	return ret;
}

void Mavlink::mavlink_update_system(void)
{
	if (!_param_initialized) {
		_param_system_id = param_find("MAV_SYS_ID");
		_param_component_id = param_find("MAV_COMP_ID");
		_param_system_type = param_find("MAV_TYPE");
		_param_use_hil_gps = param_find("MAV_USEHILGPS");
		_param_forward_externalsp = param_find("MAV_FWDEXTSP");

		/* test param - needs to be referenced, but is unused */
		(void)param_find("MAV_TEST_PAR");
	}

	/* update system and component id */
	int32_t system_id;
	param_get(_param_system_id, &system_id);

	int32_t component_id;
	param_get(_param_component_id, &component_id);


	/* only allow system ID and component ID updates
	 * after reboot - not during operation */
	if (!_param_initialized) {
		if (system_id > 0 && system_id < 255) {
			mavlink_system.sysid = system_id;
		}

		if (component_id > 0 && component_id < 255) {
			mavlink_system.compid = component_id;
		}

		_param_initialized = true;
	}

	/* warn users that they need to reboot to take this
	 * into effect
	 */
	if (system_id != mavlink_system.sysid) {
		send_statustext_critical("Save params and reboot to change SYSID");
	}

	if (component_id != mavlink_system.compid) {
		send_statustext_critical("Save params and reboot to change COMPID");
	}

	int32_t system_type;
	param_get(_param_system_type, &system_type);

	if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) {
		_system_type = system_type;
	}

	int32_t use_hil_gps;
	param_get(_param_use_hil_gps, &use_hil_gps);

	_use_hil_gps = (bool)use_hil_gps;

	int32_t forward_externalsp;
	param_get(_param_forward_externalsp, &forward_externalsp);

	_forward_externalsp = (bool)forward_externalsp;
}

int parameters_update(const struct attitude_estimator_ekf_param_handles *h, struct attitude_estimator_ekf_params *p)
{
	param_get(h->q0, &(p->q[0]));
	param_get(h->q1, &(p->q[1]));
	param_get(h->q2, &(p->q[2]));
	param_get(h->q3, &(p->q[3]));
	param_get(h->q4, &(p->q[4]));
	param_get(h->q5, &(p->q[5]));
	param_get(h->q6, &(p->q[6]));
	param_get(h->q7, &(p->q[7]));
	param_get(h->q8, &(p->q[8]));
	param_get(h->q9, &(p->q[9]));
	param_get(h->q10, &(p->q[10]));
	param_get(h->q11, &(p->q[11]));

	param_get(h->r0, &(p->r[0]));
	param_get(h->r1, &(p->r[1]));
	param_get(h->r2, &(p->r[2]));
	param_get(h->r3, &(p->r[3]));
	param_get(h->r4, &(p->r[4]));
	param_get(h->r5, &(p->r[5]));
	param_get(h->r6, &(p->r[6]));
	param_get(h->r7, &(p->r[7]));
	param_get(h->r8, &(p->r[8]));

	return OK;
}

int
MulticopterAttitudeControl::parameters_update()
{
	float v;

	/* roll gains */
	param_get(_params_handles.roll_p, &v);
	_params.att_p(0) = v;
	param_get(_params_handles.roll_rate_p, &v);
	_params.rate_p(0) = v;
	param_get(_params_handles.roll_rate_i, &v);
	_params.rate_i(0) = v;
	param_get(_params_handles.roll_rate_d, &v);
	_params.rate_d(0) = v;
	param_get(_params_handles.roll_rate_ff, &v);
	_params.rate_ff(0) = v;

	/* pitch gains */
	param_get(_params_handles.pitch_p, &v);
	_params.att_p(1) = v;
	param_get(_params_handles.pitch_rate_p, &v);
	_params.rate_p(1) = v;
	param_get(_params_handles.pitch_rate_i, &v);
	_params.rate_i(1) = v;
	param_get(_params_handles.pitch_rate_d, &v);
	_params.rate_d(1) = v;
	param_get(_params_handles.pitch_rate_ff, &v);
	_params.rate_ff(1) = v;

	/* yaw gains */
	param_get(_params_handles.yaw_p, &v);
	_params.att_p(2) = v;
	param_get(_params_handles.yaw_rate_p, &v);
	_params.rate_p(2) = v;
	param_get(_params_handles.yaw_rate_i, &v);
	_params.rate_i(2) = v;
	param_get(_params_handles.yaw_rate_d, &v);
	_params.rate_d(2) = v;
	param_get(_params_handles.yaw_rate_ff, &v);
	_params.rate_ff(2) = v;

	param_get(_params_handles.yaw_ff, &_params.yaw_ff);

	/* angular rate limits */
	param_get(_params_handles.roll_rate_max, &_params.roll_rate_max);
	_params.mc_rate_max(0) = math::radians(_params.roll_rate_max);
	param_get(_params_handles.pitch_rate_max, &_params.pitch_rate_max);
	_params.mc_rate_max(1) = math::radians(_params.pitch_rate_max);
	param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max);
	_params.mc_rate_max(2) = math::radians(_params.yaw_rate_max);

	/* manual attitude control scale */
	param_get(_params_handles.man_roll_max, &_params.man_roll_max);
	param_get(_params_handles.man_pitch_max, &_params.man_pitch_max);
	param_get(_params_handles.man_yaw_max, &_params.man_yaw_max);
	_params.man_roll_max = math::radians(_params.man_roll_max);
	_params.man_pitch_max = math::radians(_params.man_pitch_max);
	_params.man_yaw_max = math::radians(_params.man_yaw_max);

	/* manual rate control scale and auto mode roll/pitch rate limits */
	param_get(_params_handles.acro_roll_max, &v);
	_params.acro_rate_max(0) = math::radians(v);
	param_get(_params_handles.acro_pitch_max, &v);
	_params.acro_rate_max(1) = math::radians(v);
	param_get(_params_handles.acro_yaw_max, &v);
	_params.acro_rate_max(2) = math::radians(v);

	_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);

	return OK;
}

int preflight_check_main(int argc, char *argv[])
{
	bool fail_on_error = false;

	if (argc > 1 && !strcmp(argv[1], "--help")) {
		warnx("usage: preflight_check [--fail-on-error]\n\tif fail on error is enabled, will return 1 on error");
		exit(1);
	}

	if (argc > 1 && !strcmp(argv[1], "--fail-on-error")) {
		fail_on_error = true;
	}

	bool system_ok = true;

	int fd;
	/* open text message output path */
	int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
	int ret;

	/* give the system some time to sample the sensors in the background */
	usleep(150000);


	/* ---- MAG ---- */
	close(fd);
	fd = open(MAG_DEVICE_PATH, 0);
	if (fd < 0) {
		warn("failed to open magnetometer - start with 'hmc5883 start' or 'lsm303d start'");
		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: NO MAG");
		system_ok = false;
		goto system_eval;
	}
	ret = ioctl(fd, MAGIOCSELFTEST, 0);
	
	if (ret != OK) {
		warnx("magnetometer calibration missing or bad - calibrate magnetometer first");
		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CHECK/CAL");
		system_ok = false;
		goto system_eval;
	}

	/* ---- ACCEL ---- */

	close(fd);
	fd = open(ACCEL_DEVICE_PATH, 0);
	ret = ioctl(fd, ACCELIOCSELFTEST, 0);
	
	if (ret != OK) {
		warnx("accel self test failed");
		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL CHECK/CAL");
		system_ok = false;
		goto system_eval;
	}

	/* ---- GYRO ---- */

	close(fd);
	fd = open(GYRO_DEVICE_PATH, 0);
	ret = ioctl(fd, GYROIOCSELFTEST, 0);
	
	if (ret != OK) {
		warnx("gyro self test failed");
		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CHECK/CAL");
		system_ok = false;
		goto system_eval;
	}

	/* ---- BARO ---- */

	close(fd);
	fd = open(BARO_DEVICE_PATH, 0);

	/* ---- RC CALIBRATION ---- */

	param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max,
	_parameter_handles_rev, _parameter_handles_dz;

	float param_min, param_max, param_trim, param_rev, param_dz;

	bool rc_ok = true;
	char nbuf[20];

	for (int i = 0; i < 12; i++) {
		/* should the channel be enabled? */
		uint8_t count = 0;

		/* min values */
		sprintf(nbuf, "RC%d_MIN", i + 1);
		_parameter_handles_min = param_find(nbuf);
		param_get(_parameter_handles_min, &param_min);

		/* trim values */
		sprintf(nbuf, "RC%d_TRIM", i + 1);
		_parameter_handles_trim = param_find(nbuf);
		param_get(_parameter_handles_trim, &param_trim);

		/* max values */
		sprintf(nbuf, "RC%d_MAX", i + 1);
		_parameter_handles_max = param_find(nbuf);
//.........这里部分代码省略.........

int get_connect12_conf(int i_res, int i_conf, PROT prot) {
    CONNECT     connect;
    FILE        *debug_fp;
    int         i_atom;
    int         j_connect, k_connect, n_connect;
    int         j_res, j_conf, j_atom;
    int         k_lig, l_connect;
    int         n_ligs, n_conf;
    int         connect_found, lig_treated;
    RES         *res_p;
    CONF        *conf_p;
    ATOM        *atom_p,*jatom_p, *ligs[MAX_LIGS];
    int         ligs_res[MAX_LIGS];
    int         err = 0;
    
    res_p = &prot.res[i_res];
    conf_p = &prot.res[i_res].conf[i_conf];
    /* Looping over all atoms */
    for (i_atom=0; i_atom<prot.res[i_res].conf[i_conf].n_atom; i_atom++) {
        atom_p = &prot.res[i_res].conf[i_conf].atom[i_atom];
        if (!atom_p->on) continue;
        for (j_connect=0; j_connect<MAX_CONNECTED; j_connect++) {
            atom_p->connect12[j_connect] = NULL;
        }
        
        /* Error checking for connectivity parameter */
        memset(atom_p->connect12, 0, MAX_CONNECTED*sizeof(void *));
        if( param_get("CONNECT", conf_p->confName, atom_p->name, &connect) ) {
            debug_fp = fopen(env.debug_log, "a");
            fprintf(debug_fp, "   Error! get_connect12(): Can't find CONNECT parameter of conformer \"%s\" atom \"%s\"\n", conf_p->confName, atom_p->name);
            fclose(debug_fp);
            err++;
            continue;
        }
        if( connect.n > MAX_CONNECTED ) {
            debug_fp = fopen(env.debug_log, "a");
            fprintf(debug_fp, "   Error! get_connect12(): Error in CONNECT parameter for conformer \"%s\" atom \"%s\", number of connected atoms is bigger than array size\nCheck parameter file or fix mcce.h with a bigger MAX_CONNECTED \n", conf_p->confName, atom_p->name);
            fclose(debug_fp);
            return USERERR;
        }
        
        lig_treated = 0;        /* for each atom, ligand connectivy is checked at most once, no matter how many connectivities this atom has */
        /* Looping over each connectivity member */
        for (j_connect=0; j_connect<connect.n; j_connect++) {
            connect_found = 0;
            
            /* Ligand type connectivity or not */
            if (!connect.atom[j_connect].ligand) {      /* NOT ligand type */
                if (!connect.atom[j_connect].res_offset) { /* If within the same residue (off_set is 0) */
                    j_res = i_res;
                    if ( !param_get("IATOM", conf_p->confName, connect.atom[j_connect].name, &j_atom) ) {
                        /* First search atom in the same conformer */
                        j_conf = i_conf;
                        atom_p->connect12[j_connect] = &prot.res[j_res].conf[j_conf].atom[j_atom];
                        atom_p->connect12_res[j_connect] = j_res;
                        connect_found = 1;
                    }
                    else {
                        /* If not in same conformer, there are two situations: 
                        1. atom is in side chain conformer, connecting to backbone;
                        2. atom is in backbone conformer, connecting to side chain; */
                        for (j_conf = 0; j_conf < prot.res[j_res].n_conf; j_conf++) {
                            if (j_conf == i_conf) continue;
                            if ( !param_get("IATOM", prot.res[j_res].conf[j_conf].confName, connect.atom[j_connect].name, &j_atom) ) {
                                atom_p->connect12[j_connect] = &prot.res[j_res].conf[j_conf].atom[j_atom];
                                atom_p->connect12_res[j_connect] = j_res;
                                connect_found = 1;
                                break;
                            }
                        }
                        if (!connect_found) {
                            char err_msg1[MAXCHAR_LINE];
                            char err_msg2[MAXCHAR_LINE];
                            sprintf(err_msg1,"   Error! get_connect12(): connectivity of atom \"%s %s %c%04d\" is not complete",
                            atom_p->name, conf_p->confName, res_p->chainID, res_p->resSeq);
                            sprintf(err_msg2,"          get_connect12(): atom %s in the same residue is not found", connect.atom[j_connect].name);
                            if (param_exist(err_msg1, "", "")) {
                                if (param_exist(err_msg2, "", "")) {
                                    continue;
                                }
                            }

                            param_sav(err_msg1, "", "", "", 0);
                            param_sav(err_msg2, "", "", "", 0);
                                
                            debug_fp = fopen(env.debug_log, "a");
                            fprintf(debug_fp,"%s\n",err_msg1);
                            fprintf(debug_fp,"%s\n",err_msg2);
                            fclose(debug_fp);
                            err++;
                        }
                    }
                }
                else {        /* Not in the same reside. */
                    j_res = i_res + connect.atom[j_connect].res_offset;
                    if (j_res < 0 || j_res >= prot.n_res) {     /* j_res is out of residue list */
                        char err_msg1[MAXCHAR_LINE];
                        char err_msg2[MAXCHAR_LINE];
                        sprintf(err_msg1, "   Error! get_connect12(): connectivity of atom \"%s %s %c%04d\" is not complete:\n",
                        atom_p->name, res_p->resName, res_p->chainID, res_p->resSeq);
//.........这里部分代码省略.........

int
Sensors::parameters_update()
{
	bool rc_valid = true;
	float tmpScaleFactor = 0.0f;
	float tmpRevFactor = 0.0f;

	/* rc values */
	for (unsigned int i = 0; i < _rc_max_chan_count; i++) {

		param_get(_parameter_handles.min[i], &(_parameters.min[i]));
		param_get(_parameter_handles.trim[i], &(_parameters.trim[i]));
		param_get(_parameter_handles.max[i], &(_parameters.max[i]));
		param_get(_parameter_handles.rev[i], &(_parameters.rev[i]));
		param_get(_parameter_handles.dz[i], &(_parameters.dz[i]));

		tmpScaleFactor = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]);
		tmpRevFactor = tmpScaleFactor * _parameters.rev[i];

		/* handle blowup in the scaling factor calculation */
		if (!isfinite(tmpScaleFactor) ||
		    (tmpRevFactor < 0.000001f) ||
		    (tmpRevFactor > 0.2f)) {
			warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, tmpScaleFactor, (int)(_parameters.rev[i]));
			/* scaling factors do not make sense, lock them down */
			_parameters.scaling_factor[i] = 0.0f;
			rc_valid = false;

		} else {
			_parameters.scaling_factor[i] = tmpScaleFactor;
		}
	}

	/* handle wrong values */
	if (!rc_valid) {
		warnx("WARNING     WARNING     WARNING\n\nRC CALIBRATION NOT SANE!\n\n");
	}

	const char *paramerr = "FAIL PARM LOAD";

	/* channel mapping */
	if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) {
		warnx("%s", paramerr);
	}

	if (param_get(_parameter_handles.rc_map_pitch, &(_parameters.rc_map_pitch)) != OK) {
		warnx("%s", paramerr);
	}

	if (param_get(_parameter_handles.rc_map_yaw, &(_parameters.rc_map_yaw)) != OK) {
		warnx("%s", paramerr);
	}

	if (param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle)) != OK) {
		warnx("%s", paramerr);
	}

	if (param_get(_parameter_handles.rc_map_failsafe, &(_parameters.rc_map_failsafe)) != OK) {
		warnx("%s", paramerr);
	}

	if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) {
		warnx("%s", paramerr);
	}

	if (param_get(_parameter_handles.rc_map_return_sw, &(_parameters.rc_map_return_sw)) != OK) {
		warnx("%s", paramerr);
	}

	if (param_get(_parameter_handles.rc_map_posctl_sw, &(_parameters.rc_map_posctl_sw)) != OK) {
		warnx("%s", paramerr);
	}

	if (param_get(_parameter_handles.rc_map_loiter_sw, &(_parameters.rc_map_loiter_sw)) != OK) {
		warnx("%s", paramerr);
	}

	if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
		warnx("%s", paramerr);
	}

	param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1));
	param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2));
	param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3));
	param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4));
	param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5));
	param_get(_parameter_handles.rc_fails_thr, &(_parameters.rc_fails_thr));
	param_get(_parameter_handles.rc_assist_th, &(_parameters.rc_assist_th));
	_parameters.rc_assist_inv = (_parameters.rc_assist_th < 0);
	_parameters.rc_assist_th = fabs(_parameters.rc_assist_th);
	param_get(_parameter_handles.rc_auto_th, &(_parameters.rc_auto_th));
	_parameters.rc_auto_inv = (_parameters.rc_auto_th < 0);
	_parameters.rc_auto_th = fabs(_parameters.rc_auto_th);
	param_get(_parameter_handles.rc_posctl_th, &(_parameters.rc_posctl_th));
	_parameters.rc_posctl_inv = (_parameters.rc_posctl_th < 0);
	_parameters.rc_posctl_th = fabs(_parameters.rc_posctl_th);
	param_get(_parameter_handles.rc_return_th, &(_parameters.rc_return_th));
	_parameters.rc_return_inv = (_parameters.rc_return_th < 0);
	_parameters.rc_return_th = fabs(_parameters.rc_return_th);
	param_get(_parameter_handles.rc_loiter_th, &(_parameters.rc_loiter_th));
//.........这里部分代码省略.........

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

/*
 * Read specified number of accelerometer samples, calculate average and dispersion.
 */
calibrate_return read_accelerometer_avg(int sensor_correction_sub, int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num)
{
	/* get total sensor board rotation matrix */
	param_t board_rotation_h = param_find("SENS_BOARD_ROT");
	param_t board_offset_x = param_find("SENS_BOARD_X_OFF");
	param_t board_offset_y = param_find("SENS_BOARD_Y_OFF");
	param_t board_offset_z = param_find("SENS_BOARD_Z_OFF");

	float board_offset[3];
	param_get(board_offset_x, &board_offset[0]);
	param_get(board_offset_y, &board_offset[1]);
	param_get(board_offset_z, &board_offset[2]);

	math::Matrix<3, 3> board_rotation_offset;
	board_rotation_offset.from_euler(M_DEG_TO_RAD_F * board_offset[0],
			M_DEG_TO_RAD_F * board_offset[1],
			M_DEG_TO_RAD_F * board_offset[2]);

	int32_t board_rotation_int;
	param_get(board_rotation_h, &(board_rotation_int));
	enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
	math::Matrix<3, 3> board_rotation;
	get_rot_matrix(board_rotation_id, &board_rotation);

	/* combine board rotation with offset rotation */
	board_rotation = board_rotation_offset * board_rotation;

	px4_pollfd_struct_t fds[max_accel_sens];

	for (unsigned i = 0; i < max_accel_sens; i++) {
		fds[i].fd = subs[i];
		fds[i].events = POLLIN;
	}

	unsigned counts[max_accel_sens] = { 0 };
	float accel_sum[max_accel_sens][3];
	memset(accel_sum, 0, sizeof(accel_sum));

	unsigned errcount = 0;
	struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */

	/* try to get latest thermal corrections */
	if (orb_copy(ORB_ID(sensor_correction), sensor_correction_sub, &sensor_correction) != 0) {
		/* use default values */
		memset(&sensor_correction, 0, sizeof(sensor_correction));
		for (unsigned i = 0; i < 3; i++) {
			sensor_correction.accel_scale_0[i] = 1.0f;
			sensor_correction.accel_scale_1[i] = 1.0f;
			sensor_correction.accel_scale_2[i] = 1.0f;
		}
	}

	/* use the first sensor to pace the readout, but do per-sensor counts */
	while (counts[0] < samples_num) {
		int poll_ret = px4_poll(&fds[0], max_accel_sens, 1000);

		if (poll_ret > 0) {

			for (unsigned s = 0; s < max_accel_sens; s++) {
				bool changed;
				orb_check(subs[s], &changed);

				if (changed) {

					struct accel_report arp;
					orb_copy(ORB_ID(sensor_accel), subs[s], &arp);

					// Apply thermal offset corrections in sensor/board frame
					if (s == 0) {
						accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_0[0]);
						accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_0[1]);
						accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_0[2]);
					} else if (s == 1) {
						accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_1[0]);
						accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_1[1]);
						accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_1[2]);
					} else if (s == 2) {
						accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_2[0]);
						accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_2[1]);
						accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_2[2]);
					} else {
						accel_sum[s][0] += arp.x;
						accel_sum[s][1] += arp.y;
						accel_sum[s][2] += arp.z;
					}

					counts[s]++;
				}
			}

		} else {
			errcount++;
			continue;
		}

		if (errcount > samples_num / 10) {
			return calibrate_return_error;
//.........这里部分代码省略.........

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

int
MulticopterAttitudeControl::parameters_update()
{
	float v;

	float roll_tc, pitch_tc;

	param_get(_params_handles.roll_tc, &roll_tc);
	param_get(_params_handles.pitch_tc, &pitch_tc);

	/* roll gains */
	param_get(_params_handles.roll_p, &v);
	_params.att_p(0) = v * (ATTITUDE_TC_DEFAULT / roll_tc);
	param_get(_params_handles.roll_rate_p, &v);
	_params.rate_p(0) = v * (ATTITUDE_TC_DEFAULT / roll_tc);
	param_get(_params_handles.roll_rate_i, &v);
	_params.rate_i(0) = v;
	param_get(_params_handles.roll_rate_d, &v);
	_params.rate_d(0) = v * (ATTITUDE_TC_DEFAULT / roll_tc);
	param_get(_params_handles.roll_rate_ff, &v);
	_params.rate_ff(0) = v;

	/* pitch gains */
	param_get(_params_handles.pitch_p, &v);
	_params.att_p(1) = v * (ATTITUDE_TC_DEFAULT / pitch_tc);
	param_get(_params_handles.pitch_rate_p, &v);
	_params.rate_p(1) = v * (ATTITUDE_TC_DEFAULT / pitch_tc);
	param_get(_params_handles.pitch_rate_i, &v);
	_params.rate_i(1) = v;
	param_get(_params_handles.pitch_rate_d, &v);
	_params.rate_d(1) = v * (ATTITUDE_TC_DEFAULT / pitch_tc);
	param_get(_params_handles.pitch_rate_ff, &v);
	_params.rate_ff(1) = v;

	/* yaw gains */
	param_get(_params_handles.yaw_p, &v);
	_params.att_p(2) = v;
	param_get(_params_handles.yaw_rate_p, &v);
	_params.rate_p(2) = v;
	param_get(_params_handles.yaw_rate_i, &v);
	_params.rate_i(2) = v;
	param_get(_params_handles.yaw_rate_d, &v);
	_params.rate_d(2) = v;
	param_get(_params_handles.yaw_rate_ff, &v);
	_params.rate_ff(2) = v;

	param_get(_params_handles.yaw_ff, &_params.yaw_ff);

	/* angular rate limits */
	param_get(_params_handles.roll_rate_max, &_params.roll_rate_max);
	_params.mc_rate_max(0) = math::radians(_params.roll_rate_max);
	param_get(_params_handles.pitch_rate_max, &_params.pitch_rate_max);
	_params.mc_rate_max(1) = math::radians(_params.pitch_rate_max);
	param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max);
	_params.mc_rate_max(2) = math::radians(_params.yaw_rate_max);

	/* manual rate control scale and auto mode roll/pitch rate limits */
	param_get(_params_handles.acro_roll_max, &v);
	_params.acro_rate_max(0) = math::radians(v);
	param_get(_params_handles.acro_pitch_max, &v);
	_params.acro_rate_max(1) = math::radians(v);
	param_get(_params_handles.acro_yaw_max, &v);
	_params.acro_rate_max(2) = math::radians(v);

	/* stick deflection needed in rattitude mode to control rates not angles */
	param_get(_params_handles.rattitude_thres, &_params.rattitude_thres);

	param_get(_params_handles.vtol_type, &_params.vtol_type);

	_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);

	return OK;
}

int main(int argc, char** argv)
{