C++ param_find函数代码示例

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

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

static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_status)
{
	bool success = true; // start with a pass and change to a fail if any test fails
	float test_limit = 1.0f; // pass limit re-used for each test

	// Get sensor_preflight data if available and exit with a fail recorded if not
	int sensors_sub = orb_subscribe(ORB_ID(sensor_preflight));
	sensor_preflight_s sensors = {};

	if (orb_copy(ORB_ID(sensor_preflight), sensors_sub, &sensors) != PX4_OK) {
		goto out;
	}

	// Use the difference between IMU's to detect a bad calibration.
	// If a single IMU is fitted, the value being checked will be zero so this check will always pass.
	param_get(param_find("COM_ARM_IMU_ACC"), &test_limit);

	if (sensors.accel_inconsistency_m_s_s > test_limit) {
		if (report_status) {
			mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCELS INCONSISTENT - CHECK CAL");
		}

		success = false;
		goto out;

	} else if (sensors.accel_inconsistency_m_s_s > test_limit * 0.8f) {
		if (report_status) {
			mavlink_log_info(mavlink_log_pub, "PREFLIGHT ADVICE: ACCELS INCONSISTENT - CHECK CAL");
		}
	}

	// Fail if gyro difference greater than 5 deg/sec and notify if greater than 2.5 deg/sec
	param_get(param_find("COM_ARM_IMU_GYR"), &test_limit);

	if (sensors.gyro_inconsistency_rad_s > test_limit) {
		if (report_status) {
			mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYROS INCONSISTENT - CHECK CAL");
		}

		success = false;
		goto out;

	} else if (sensors.gyro_inconsistency_rad_s > test_limit * 0.5f) {
		if (report_status) {
			mavlink_log_info(mavlink_log_pub, "PREFLIGHT ADVICE: GYROS INCONSISTENT - CHECK CAL");
		}
	}

out:
	orb_unsubscribe(sensors_sub);
	return success;
}

void
Syslink::update_params(bool force_set)
{
	param_t _param_radio_channel = param_find("SLNK_RADIO_CHAN");
	param_t _param_radio_rate = param_find("SLNK_RADIO_RATE");
	param_t _param_radio_addr1 = param_find("SLNK_RADIO_ADDR1");
	param_t _param_radio_addr2 = param_find("SLNK_RADIO_ADDR2");


	// reading parameter values into temp variables

	int32_t channel, rate, addr1, addr2;
	uint64_t addr = 0;

	param_get(_param_radio_channel, &channel);
	param_get(_param_radio_rate, &rate);
	param_get(_param_radio_addr1, &addr1);
	param_get(_param_radio_addr2, &addr2);

	memcpy(&addr, &addr2, 4);
	memcpy(((char *)&addr) + 4, &addr1, 4);


	hrt_abstime t = hrt_absolute_time();

	// request updates if needed

	if (channel != this->_channel || force_set) {
		this->_channel = channel;
		set_channel(channel);
		this->_params_update[0] = t;
		this->_params_ack[0] = 0;
	}

	if (rate != this->_rate || force_set) {
		this->_rate = rate;
		set_datarate(rate);
		this->_params_update[1] = t;
		this->_params_ack[1] = 0;
	}

	if (addr != this->_addr || force_set) {
		this->_addr = addr;
		set_address(addr);
		this->_params_update[2] = t;
		this->_params_ack[2] = 0;
	}


}

InputMavlinkCmdMount::InputMavlinkCmdMount(bool stabilize)
	: _stabilize {stabilize, stabilize, stabilize}
{
	param_t handle = param_find("MAV_SYS_ID");

	if (handle != PARAM_INVALID) {
		param_get(handle, &_mav_sys_id);
	}

	handle = param_find("MAV_COMP_ID");

	if (handle != PARAM_INVALID) {
		param_get(handle, &_mav_comp_id);
	}
}

int TemperatureCalibrationBaro::finish_sensor_instance(PerSensorData &data, int sensor_index)
{
	if (!data.hot_soaked || data.tempcal_complete) {
		return 0;
	}

	double res[POLYFIT_ORDER + 1] = {};
	data.P[0].fit(res);
	res[POLYFIT_ORDER] =
		0.0; // normalise the correction to be zero at the reference temperature by setting the X^0 coefficient to zero
	PX4_INFO("Result baro %u %.20f %.20f %.20f %.20f %.20f %.20f", sensor_index, (double)res[0],
		 (double)res[1], (double)res[2], (double)res[3], (double)res[4], (double)res[5]);
	data.tempcal_complete = true;

	char str[30];
	float param = 0.0f;
	int result = PX4_OK;

	set_parameter("TC_B%d_ID", sensor_index, &data.device_id);

	for (unsigned coef_index = 0; coef_index <= POLYFIT_ORDER; coef_index++) {
		sprintf(str, "TC_B%d_X%d", sensor_index, (POLYFIT_ORDER - coef_index));
		param = (float)res[coef_index];
		result = param_set_no_notification(param_find(str), &param);

		if (result != PX4_OK) {
			PX4_ERR("unable to reset %s", str);
		}
	}

	set_parameter("TC_B%d_TMAX", sensor_index, &data.high_temp);
	set_parameter("TC_B%d_TMIN", sensor_index, &data.low_temp);
	set_parameter("TC_B%d_TREF", sensor_index, &data.ref_temp);
	return 0;
}

void CameraInterface::get_pins()
{

	// Get parameter handle
	_p_pin = param_find("TRIG_PINS");

	int pin_list;
	param_get(_p_pin, &pin_list);

	// Set all pins as invalid
	for (unsigned i = 0; i < arraySize(_pins); i++) {
		_pins[i] = -1;
	}

	// Convert number to individual channels
	unsigned i = 0;
	int single_pin;

	while ((single_pin = pin_list % 10)) {

		_pins[i] = single_pin - 1;

		if (_pins[i] < 0) {
			_pins[i] = -1;
		}

		pin_list /= 10;
		i++;
	}

}

BlockParamBase::BlockParamBase(Block *parent, const char *name, bool parent_prefix) :
	_handle(PARAM_INVALID)
{
	char fullname[blockNameLengthMax];

	if (parent == NULL) {
		strncpy(fullname, name, blockNameLengthMax);

	} else {
		char parentName[blockNameLengthMax];
		parent->getName(parentName, blockNameLengthMax);

		if (!strcmp(name, "")) {
			strncpy(fullname, parentName, blockNameLengthMax);
			// ensure string is terminated
			fullname[sizeof(fullname) - 1] = '\0';

		} else if (parent_prefix) {
			snprintf(fullname, blockNameLengthMax, "%s_%s", parentName, name);

		} else {
			strncpy(fullname, name, blockNameLengthMax);
			// ensure string is terminated
			fullname[sizeof(fullname) - 1] = '\0';
		}

		parent->getParams().add(this);
	}

	_handle = param_find(fullname);

	if (_handle == PARAM_INVALID) {
		printf("error finding param: %s\n", fullname);
	}
};

bool circuit_breaker_enabled(const char* breaker, int32_t magic)
{
	int32_t val;
	(void)param_get(param_find(breaker), &val);

	return (val == magic);
}

Standard::Standard(VtolAttitudeControl *attc) :
	VtolType(attc),
	_flag_enable_mc_motors(true),
	_pusher_throttle(0.0f),
	_mc_att_ctl_weight(1.0f),
	_airspeed_trans_blend_margin(0.0f)
{
	_vtol_schedule.flight_mode = MC_MODE;
	_vtol_schedule.transition_start = 0;

	_params_handles_standard.front_trans_dur = param_find("VT_F_TRANS_DUR");
	_params_handles_standard.back_trans_dur = param_find("VT_B_TRANS_DUR");
	_params_handles_standard.pusher_trans = param_find("VT_TRANS_THR");
	_params_handles_standard.airspeed_blend = param_find("VT_ARSP_BLEND");
	_params_handles_standard.airspeed_trans = param_find("VT_ARSP_TRANS");
 }

int
test_param(int argc, char *argv[])
{
	param_t		p;

	p = param_find("test");
	if (p == PARAM_INVALID)
		errx(1, "test parameter not found");

	param_type_t t = param_type(p);
	if (t != PARAM_TYPE_INT32)
		errx(1, "test parameter type mismatch (got %u)", (unsigned)t);

	int32_t	val;
	if (param_get(p, &val) != 0)
		errx(1, "failed to read test parameter");
	if (val != 0x12345678)
		errx(1, "parameter value mismatch (val = %X)", val);

	val = 0xa5a5a5a5;
	if (param_set(p, &val) != 0)
		errx(1, "failed to write test parameter");
	if (param_get(p, &val) != 0)
		errx(1, "failed to re-read test parameter");
	if ((uint32_t)val != 0xa5a5a5a5)
		errx(1, "parameter value mismatch after write (val = %X)", val);

	warnx("parameter test PASS");

	return 0;
}

void GPS::initializeCommunicationDump()
{
	param_t gps_dump_comm_ph = param_find("GPS_DUMP_COMM");
	int32_t param_dump_comm;

	if (gps_dump_comm_ph == PARAM_INVALID || param_get(gps_dump_comm_ph, &param_dump_comm) != 0) {
		return;
	}

	if (param_dump_comm != 1) {
		return; //dumping disabled
	}

	_dump_from_device = new gps_dump_s();
	_dump_to_device = new gps_dump_s();

	if (!_dump_from_device || !_dump_to_device) {
		PX4_ERR("failed to allocated dump data");
		return;
	}

	memset(_dump_to_device, 0, sizeof(gps_dump_s));
	memset(_dump_from_device, 0, sizeof(gps_dump_s));

	int instance;
	//make sure to use a large enough queue size, so that we don't lose messages. You may also want
	//to increase the logger rate for that.
	_dump_communication_pub = orb_advertise_multi_queue(ORB_ID(gps_dump), _dump_from_device, &instance,
				  ORB_PRIO_DEFAULT, 8);
}

CameraInterfacePWM::CameraInterfacePWM():
	CameraInterface(),
	_camera_is_on(false)
{
	_p_pin = param_find("TRIG_PINS");
	int pin_list;
	param_get(_p_pin, &pin_list);

	// Set all pins as invalid
	for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
		_pins[i] = -1;
	}

	// Convert number to individual channels
	unsigned i = 0;
	int single_pin;

	while ((single_pin = pin_list % 10)) {

		_pins[i] = single_pin - 1;

		if (_pins[i] < 0) {
			_pins[i] = -1;
		}

		pin_list /= 10;
		i++;
	}

	setup();
}

int parameters_init(struct param_handles *h)
{
	/* PID parameters */
	h->yaw_p 	=	param_find("RV_YAW_P");

	return OK;
}

// return false if the magnetomer measurements are inconsistent
static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_status)
{
	// get the sensor preflight data
	int sensors_sub = orb_subscribe(ORB_ID(sensor_preflight));
	struct sensor_preflight_s sensors = {};

	if (orb_copy(ORB_ID(sensor_preflight), sensors_sub, &sensors) != 0) {
		// can happen if not advertised (yet)
		return true;
	}

	orb_unsubscribe(sensors_sub);

	// Use the difference between sensors to detect a bad calibration, orientation or magnetic interference.
	// If a single sensor is fitted, the value being checked will be zero so this check will always pass.
	float test_limit;
	param_get(param_find("COM_ARM_MAG"), &test_limit);

	if (sensors.mag_inconsistency_ga > test_limit) {
		if (report_status) {
			mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: MAG SENSORS INCONSISTENT");
		}

		return false;
	}

	return true;
}

FixedwingLandDetector::FixedwingLandDetector() : LandDetector(),
	_paramHandle(),
	_params(),
	_vehicleLocalPositionSub(-1),
	_vehicleLocalPosition({}),
	_airspeedSub(-1),
	_airspeed({}),
	_parameterSub(-1),
	_velocity_xy_filtered(0.0f),
	_velocity_z_filtered(0.0f),
	_airspeed_filtered(0.0f),
	_landDetectTrigger(0)
{
	_paramHandle.maxVelocity = param_find("LNDFW_VEL_XY_MAX");
	_paramHandle.maxClimbRate = param_find("LNDFW_VEL_Z_MAX");
	_paramHandle.maxAirSpeed = param_find("LNDFW_AIRSPD_MAX");
}