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

int uavcannode_start(int argc, char *argv[])
{
	resources("Before board_app_initialize");

	board_app_initialize(NULL);

	resources("After board_app_initialize");

	// CAN bitrate
	int32_t bitrate = 0;
	// Node ID
	int32_t node_id = 0;

	// Did the bootloader auto baud and get a node ID Allocated

	bootloader_app_shared_t shared;
	int valid  = bootloader_app_shared_read(&shared, BootLoader);

	if (valid == 0) {

		bitrate = shared.bus_speed;
		node_id = shared.node_id;

		// Invalidate to prevent deja vu

		bootloader_app_shared_invalidate();

	} else {

		// Node ID
		(void)param_get(param_find("CANNODE_NODE_ID"), &node_id);
		(void)param_get(param_find("CANNODE_BITRATE"), &bitrate);
	}

	if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) {
		warnx("Invalid Node ID %i", node_id);
		::exit(1);
	}

	// Start
	warnx("Node ID %u, bitrate %u", node_id, bitrate);
	int rv = UavcanNode::start(node_id, bitrate);
	resources("After UavcanNode::start");
	::sleep(1);
	return rv;
}

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

MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
	_paramHandle(),
	_params(),
	_vehicleLocalPositionSub(-1),
	_actuatorsSub(-1),
	_armingSub(-1),
	_parameterSub(-1),
	_attitudeSub(-1),
	_manualSub(-1),
	_vehicleLocalPosition{},
	_actuators{},
	_arming{},
	_vehicleAttitude{},
	_ctrl_state{},
	_landTimer(0),
	_freefallTimer(0)
{
	_paramHandle.maxRotation = param_find("LNDMC_ROT_MAX");
	_paramHandle.maxVelocity = param_find("LNDMC_XY_VEL_MAX");
	_paramHandle.maxClimbRate = param_find("LNDMC_Z_VEL_MAX");
	_paramHandle.maxThrottle = param_find("MPC_THR_MIN");
	_paramHandle.minManThrottle = param_find("MPC_MANTHR_MIN");
	_paramHandle.acc_threshold_m_s2 = param_find("LNDMC_FFALL_THR");
	_paramHandle.ff_trigger_time = param_find("LNDMC_FFALL_TTRI");
}

LandingTargetEstimator::LandingTargetEstimator() :
	_targetPosePub(nullptr),
	_targetInnovationsPub(nullptr),
	_paramHandle(),
	_vehicleLocalPosition_valid(false),
	_vehicleAttitude_valid(false),
	_sensorBias_valid(false),
	_new_irlockReport(false),
	_estimator_initialized(false),
	_faulty(false),
	_last_predict(0),
	_last_update(0)
{
	_paramHandle.acc_unc = param_find("LTEST_ACC_UNC");
	_paramHandle.meas_unc = param_find("LTEST_MEAS_UNC");
	_paramHandle.pos_unc_init = param_find("LTEST_POS_UNC_IN");
	_paramHandle.vel_unc_init = param_find("LTEST_VEL_UNC_IN");
	_paramHandle.mode = param_find("LTEST_MODE");
	_paramHandle.scale_x = param_find("LTEST_SCALE_X");
	_paramHandle.scale_y = param_find("LTEST_SCALE_Y");

	// Initialize uORB topics.
	_initialize_topics();

	_check_params(true);
}

MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
	_paramHandle(),
	_params(),
	_vehicleLocalPositionSub(-1),
	_actuatorsSub(-1),
	_armingSub(-1),
	_attitudeSub(-1),
	_manualSub(-1),
	_ctrl_state_sub(-1),
	_vehicle_control_mode_sub(-1),
	_vehicleLocalPosition{},
	_actuators{},
	_arming{},
	_vehicleAttitude{},
	_manual{},
	_ctrl_state{},
	_ctrl_mode{},
	_min_trust_start(0),
	_arming_time(0)
{
	_paramHandle.maxRotation = param_find("LNDMC_ROT_MAX");
	_paramHandle.maxVelocity = param_find("LNDMC_XY_VEL_MAX");
	_paramHandle.maxClimbRate = param_find("LNDMC_Z_VEL_MAX");
	_paramHandle.minThrottle = param_find("MPC_THR_MIN");
	_paramHandle.minManThrottle = param_find("MPC_MANTHR_MIN");
	_paramHandle.freefall_acc_threshold = param_find("LNDMC_FFALL_THR");
	_paramHandle.freefall_trigger_time = param_find("LNDMC_FFALL_TTRI");
}