int
set_hil_on_off(bool hil_enabled)
{
	int ret = OK;

	/* Enable HIL */
	if (hil_enabled && !mavlink_hil_enabled) {

		/* Advertise topics */
		pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
		pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);

		/* sensore level hil */
		pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
		pub_hil_gps = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);

		mavlink_hil_enabled = true;

		/* ramp up some HIL-related subscriptions */
		unsigned hil_rate_interval;

		if (baudrate < 19200) {
			/* 10 Hz */
			hil_rate_interval = 100;

		} else if (baudrate < 38400) {
			/* 10 Hz */
			hil_rate_interval = 100;

		} else if (baudrate < 115200) {
			/* 20 Hz */
			hil_rate_interval = 50;

		} else {
			/* 200 Hz */
			hil_rate_interval = 5;
		}

		orb_set_interval(mavlink_subs.spa_sub, hil_rate_interval);
		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval);
	}

	if (!hil_enabled && mavlink_hil_enabled) {
		mavlink_hil_enabled = false;
		orb_set_interval(mavlink_subs.spa_sub, 200);

	} else {
		ret = ERROR;
	}

	return ret;
}

int
set_hil_on_off(bool hil_enabled)
{
	int ret = OK;

	/* Enable HIL */
	if (hil_enabled && !mavlink_hil_enabled) {

		//printf("\n HIL ON \n");

		/* Advertise topics */
		pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
		pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);

		printf("\n pub_hil_attitude :%i\n", pub_hil_attitude);
		printf("\n pub_hil_global_pos :%i\n", pub_hil_global_pos);

		mavlink_hil_enabled = true;

		/* ramp up some HIL-related subscriptions */
		unsigned hil_rate_interval;

		if (baudrate < 19200) {
			/* 10 Hz */
			hil_rate_interval = 100;
		} else if (baudrate < 38400) {
			/* 10 Hz */
			hil_rate_interval = 100;
		} else if (baudrate < 115200) {
			/* 20 Hz */
			hil_rate_interval = 50;
		} else if (baudrate < 460800) {
			/* 50 Hz */
			hil_rate_interval = 20;
		} else {
			/* 100 Hz */
			hil_rate_interval = 10;
		}

		orb_set_interval(mavlink_subs.spa_sub, hil_rate_interval);
	}

	if (!hil_enabled && mavlink_hil_enabled) {
		mavlink_hil_enabled = false;
		orb_set_interval(mavlink_subs.spa_sub, 200);

	} else {
		ret = ERROR;
	}

	return ret;
}

/**
 * Main execution thread
 */
int unibo_ECF_standard_thread_main(int argc, char *argv[])
{

    warnx("[unibo_ECF_standard] starting\n");

    thread_running = true;

    warnx("Hello Sky!\n");
    model = ECF_stand_q(); //Init model!

    /* subscribe sensor measurements from sensor_combined  */
    struct sensor_combined_s sens_mes;
    int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
    /* set data to 1Hz */
    orb_set_interval(sensor_sub_fd, 3); //1000 = 1Hz (ms)


    /* advertise attitude topic */
    struct vehicle_attitude_s ECF_out;
    memset(&ECF_out, 0, sizeof(ECF_out));
    int att_pub_fd = orb_advertise(ORB_ID(vehicle_attitude), &ECF_out);

    /* one could wait for multiple topics with this technique, just using one here */
    struct pollfd fds[] = {
        { .fd = sensor_sub_fd,   .events = POLLIN },
        /* there could be more file descriptors here, in the form like:
         * { .fd = other_sub_fd,   .events = POLLIN },
         */
    };

void simpleapp_task() {
	printf("Initializing /dev/ttyS2...\n");

	int uartfd = open("/dev/ttyS2", O_RDWR | O_NOCTTY); // TELEM2 port
	assert(uartfd >= 0);
	// manage terminal settings
	int termios_state_ttyS2;
	struct termios existing_config_ttyS2;
	// get existing terminal config and store it.
	assert((termios_state_ttyS2 = tcgetattr(uartfd, &existing_config_ttyS2)) >= 0);
	struct termios config_ttyS2;
	// duplicate into the new config
	tcgetattr(uartfd, &config_ttyS2);
	// memcpy(config_ttyS2, existing_config_ttyS2);

	// clear ONLCR flag
	config_ttyS2.c_oflag &= ~ONLCR;
	// set baud rate
	assert(cfsetispeed(&config_ttyS2, B921600) >= 0 || cfsetospeed(&config_ttyS2, B921600) >= 0);
	// go ahead and set the config i am setting up
	assert((termios_state_ttyS2 = tcsetattr(uartfd, TCSANOW, &config_ttyS2)) >= 0);

	/* subscribe to sensor_combined topic */
	int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
	orb_set_interval(sensor_sub_fd, 2);

	/* one could wait for multiple topics with this technique, just using one here */
	struct pollfd fds[] = {
		{ .fd = sensor_sub_fd,   .events = POLLIN },
		/* there could be more file descriptors here, in the form like:
		{ .fd = other_sub_fd,   .events = POLLIN },
		*/
	};

/**
 * Main loop function
 */
void
PCA9685::i2cpwm()
{
	if (_mode == IOX_MODE_TEST_OUT) {
		setPin(0, PCA9685_PWMCENTER);
		_should_run = true;

	} else if (_mode == IOX_MODE_OFF) {
		_should_run = false;

	} else {
		if (!_mode_on_initialized) {
			/* Subscribe to actuator control 2 (payload group for gimbal) */
			_actuator_controls_sub = orb_subscribe(ORB_ID(actuator_controls_2));
			/* set the uorb update interval lower than the driver pwm interval */
			orb_set_interval(_actuator_controls_sub, 1000.0f / PCA9685_PWMFREQ - 5);

			_mode_on_initialized = true;
		}

		/* Read the servo setpoints from the actuator control topics (gimbal) */
		bool updated;
		orb_check(_actuator_controls_sub, &updated);

		if (updated) {
			orb_copy(ORB_ID(actuator_controls_2), _actuator_controls_sub, &_actuator_controls);

			for (int i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
				/* Scale the controls to PWM, first multiply by pi to get rad,
				 * the control[i] values are on the range -1 ... 1 */
				uint16_t new_value = PCA9685_PWMCENTER +
						     (_actuator_controls.control[i] * M_PI_F * PCA9685_SCALE);
				DEVICE_DEBUG("%d: current: %u, new %u, control %.2f", i, _current_values[i], new_value,
					     (double)_actuator_controls.control[i]);

				if (new_value != _current_values[i] &&
				    isfinite(new_value) &&
				    new_value >= PCA9685_PWMMIN &&
				    new_value <= PCA9685_PWMMAX) {
					/* This value was updated, send the command to adjust the PWM value */
					setPin(i, new_value);
					_current_values[i] = new_value;
				}
			}
		}

		_should_run = true;
	}

	// check if any activity remains, else stop
	if (!_should_run) {
		_running = false;
		return;
	}

	// re-queue ourselves to run again later
	_running = true;
	work_queue(LPWORK, &_work, (worker_t)&PCA9685::i2cpwm_trampoline, this, _i2cpwm_interval);
}

Subscription<T>::Subscription(
	List<SubscriptionBase *> * list,
	const struct orb_metadata *meta, unsigned interval) :
	T(), // initialize data structure to zero
	SubscriptionBase(list, meta) {
	setHandle(orb_subscribe(getMeta()));
	orb_set_interval(getHandle(), interval);
}

int att_read_simple_main(int argc, char *argv[]) {
    PX4_INFO("Hello Sky!");
    
    int att = orb_subscribe(ORB_ID(vehicle_attitude));
    orb_set_interval(att, 1000);

    px4_pollfd_struct_t fds[] = {
            { .fd = att,   .events = POLLIN },
    };

// thread principale con loop
int unibo_motor_output_thread_main(int argc, char *argv[])
{
	int count=0;
	warnx("[unibo_motor_output] starting\n");
	unibomo_thread_running = true;

	// subscribe a ORB
	int motor_output_fd = orb_subscribe(ORB_ID(motor_output));
	orb_set_interval(motor_output_fd, 3);

	struct pollfd fds[] = { { .fd = motor_output_fd, .events = POLLIN } };

int test_main(int argc, char *argv[])
{
	printf("Hello There!\n");

    // TESTING for ADC input
    int adc_prox_sub_fd = orb_subscribe(ORB_ID(adc_prox));
    orb_set_interval(adc_prox_sub_fd, 100);

    struct pollfd fds[] = {
        {.fd=adc_prox_sub_fd,   .events =POLLIN},
    };

int InputMavlinkCmdMount::initialize()
{
	if ((_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command))) < 0) {
		return -errno;
	}

	// rate-limit inputs to 100Hz. If we don't do this and the output is configured to mavlink mode,
	// it will publish vehicle_command's as well, causing the input poll() in here to return
	// immediately, which in turn will cause an output update and thus a busy loop.
	orb_set_interval(_vehicle_command_sub, 10);

	return 0;
}

int uuv_example_app_main(int argc, char *argv[])
{
	PX4_INFO("auv_hippocampus_example_app has been started!");

	/* subscribe to sensor_combined topic */
	int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
	/* limit the update rate to 5 Hz */
	orb_set_interval(sensor_sub_fd, 200);

	/* subscribe to control_state topic */
	int vehicle_attitude_sub_fd = orb_subscribe(ORB_ID(vehicle_attitude));
	/* limit the update rate to 5 Hz */
	orb_set_interval(vehicle_attitude_sub_fd, 200);

	/* advertise to actuator_control topic */
	struct actuator_controls_s act;
	memset(&act, 0, sizeof(act));
	orb_advert_t act_pub = orb_advertise(ORB_ID(actuator_controls_0), &act);

	/* one could wait for multiple topics with this technique, just using one here */
	px4_pollfd_struct_t fds[] = {
		{ .fd = sensor_sub_fd,   .events = POLLIN },
		{ .fd = vehicle_attitude_sub_fd,   .events = POLLIN },

void listener(const orb_id_t &id, unsigned num_msgs, unsigned topic_instance, unsigned topic_interval)
{
	if (orb_exists(id, topic_instance) != 0) {
		printf("never published\n");
		return;
	}

	int sub = orb_subscribe_multi(id, topic_instance);
	orb_set_interval(sub, topic_interval);

	bool updated = false;
	unsigned i = 0;
	hrt_abstime start_time = hrt_absolute_time();

	while (i < num_msgs) {
		orb_check(sub, &updated);

		if (i == 0) {
			updated = true;

		} else {
			usleep(500);
		}

		if (updated) {
			start_time = hrt_absolute_time();
			i++;

			printf("\nTOPIC: %s instance %d #%d\n", id->o_name, topic_instance, i);

			T container;

			if (orb_copy(id, sub, &container) == PX4_OK) {
				print_message(container);

			} else {
				PX4_ERR("orb_copy failed");
			}

		} else {
			if (hrt_elapsed_time(&start_time) > 2 * 1000 * 1000) {
				printf("Waited for 2 seconds without a message. Giving up.\n");
				break;
			}
		}
	}

	orb_unsubscribe(sub);
}

int helloSky_main(int argc, char *argv[])
{
	printf("Hello Sky!\n");
 
	/* subscribe to sensor_combined topic */
	int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
	// set up the discretization time 
	orb_set_interval(sensor_sub_fd, 1000);
 
	/* one could wait for multiple topics with this technique, just using one here */
	struct pollfd fds[] = {
		{ .fd = sensor_sub_fd,   .events = POLLIN },
		/* there could be more file descriptors here, in the form like:
		 * { .fd = other_sub_fd,   .events = POLLIN },
		 */
	};

SubscriptionBase::SubscriptionBase(const struct orb_metadata *meta,
				   unsigned interval, unsigned instance) :
	_meta(meta),
	_instance(instance),
	_handle()
{
	if (_instance > 0) {
		_handle =  orb_subscribe_multi(
				   getMeta(), instance);

	} else {
		_handle =  orb_subscribe(getMeta());
	}

	if (_handle < 0) { warnx("sub failed"); }

	orb_set_interval(getHandle(), interval);
}

int px4_simple_app_main(int argc, char *argv[])
{
	printf("Hello Sky!\n");

	/* subscribe to sensor_combined topic */
	int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
	orb_set_interval(sensor_sub_fd, 1000);

	/* advertise attitude topic */
	struct vehicle_attitude_s att;
	memset(&att, 0, sizeof(att));
	orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);

	/* one could wait for multiple topics with this technique, just using one here */
	struct pollfd fds[] = {
		{ .fd = sensor_sub_fd,   .events = POLLIN },
		/* there could be more file descriptors here, in the form like:
		 * { .fd = other_sub_fd,   .events = POLLIN },
		 */
	};

int wastegate_ctrl_main(int argc, char *argv[])
{
	PX4_INFO("Starting Fireblade Wastegate Control Application");

	/* subscribe to sensor_combined topic */
	int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_wastegate));
	orb_set_interval(sensor_sub_fd, 1000);

	/* advertise attitude topic */
	struct actuator_wastegate_s act;
	memset(&act, 0, sizeof(act));
	orb_advert_t act_pub = orb_advertise(ORB_ID(actuator_wastegate), &act);

	/* one could wait for multiple topics with this technique, just using one here */
	px4_pollfd_struct_t fds[] = {
		{ .fd = sensor_sub_fd,   .events = POLLIN },
		/* there could be more file descriptors here, in the form like:
		 * { .fd = other_sub_fd,   .events = POLLIN },
		 */
	};

static int rpgRateControllerThreadMain(int argc, char *argv[])
{
  struct imu_msg_s imu_msg;
  memset(&imu_msg, 0, sizeof(imu_msg));
  struct offboard_control_setpoint_s offboard_sp;
  memset(&offboard_sp, 0, sizeof(offboard_sp));
  struct offboard_control_setpoint_s laird_sp;
  memset(&laird_sp, 0, sizeof(laird_sp));
  struct torques_and_thrust_s desired_torques_and_thrust;
  memset(&desired_torques_and_thrust, 0, sizeof(desired_torques_and_thrust));

  int param_sub = orb_subscribe(ORB_ID(parameter_update));
  int offboard_setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
  int laird_sub = orb_subscribe(ORB_ID(laird_control_setpoint));
  int imu_sub = orb_subscribe(ORB_ID(imu_msg));

  orb_advert_t rotor_thrusts_pub = orb_advertise(ORB_ID(torques_and_thrust), &desired_torques_and_thrust);

  // Limit this loop frequency to 200Hz
  orb_set_interval(imu_sub, 5);

  struct pollfd fds[2] = { {.fd = imu_sub, .events = POLLIN}, {.fd = param_sub, .events = POLLIN}};

int px4_simple_main(int argc, char *argv[])
{
    PX4_INFO("Hello Sky!");

    //ORB_DECLARE(distance_around);
    /* subscribe to sensor_combined topic */
    int sonardata_sub_fd = orb_subscribe(ORB_ID(SonarGroupDistance));
    orb_set_interval(sonardata_sub_fd, 1000);

    /* advertise attitude topic */
    struct SonarGroupDistance_s sonardata;

    memset(&sonardata, 0, sizeof(sonardata));

    /* one could wait for multiple topics with this technique, just using one here */
    px4_pollfd_struct_t fds[] = {
        { .fd = sonardata_sub_fd,   .events = POLLIN },
        //      { .fd = distancearound_sub_fd,   .events = POLLIN },
        /* there could be more file descriptors here, in the form like:
                                 * { .fd = other_sub_fd,   .events = POLLIN },
                                 */
    };

/**
 * Main execution thread
 */
int unibo_trajectory_ref_thread_main(int argc, char *argv[])
//int simple_test_app_main(int argc, char *argv[])
{
	warnx("[unibo_trajectory_ref] starting\n");

	thread_running = true;

	model = TRAJECTORY_GENERATOR_APP(); //Init model!

	/* subscribe to attitude topic */
	int vehicle_attitude_fd = orb_subscribe(ORB_ID(vehicle_attitude));

	/* subscribe to joystick topic */
	int joystick_fd = orb_subscribe(ORB_ID(unibo_joystick));
	orb_set_interval(joystick_fd, 10); //1000 = 1Hz (ms)   100 HZ!!

	/* subscribe to position topic */
	int local_position_fd = orb_subscribe(ORB_ID(vehicle_local_position));

	/* subscribe to parameters topic */
	int unibo_parameters_fd = orb_subscribe(ORB_ID(unibo_parameters));


	/* advertise reference topic */
	struct unibo_reference_s reference;
	memset(&reference, 0, sizeof(reference));
	int reference_pub_fd = orb_advertise(ORB_ID(unibo_reference), &reference);

	/* advertise local_pos_setpoint topic */
	struct vehicle_local_position_setpoint_s setpoint;
	memset(&setpoint, 0, sizeof(setpoint));
	int setpoint_pub_fd = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &setpoint);

	/* advertise local_pos_setpoint topic */
	struct position_setpoint_triplet_s setpoint_triplet;
	memset(&setpoint_triplet, 0, sizeof(setpoint_triplet));
	int setpoint_triplet_pub_fd = orb_advertise(ORB_ID(position_setpoint_triplet), &setpoint_triplet);

	/* one could wait for multiple topics with this technique, just using one here */
//	struct pollfd fds[] = {
//		{ .fd = joystick_fd,   .events = POLLIN },                                //CAMBIARE in joystick
//		/* there could be more file descriptors here, in the form like:
//		 * { .fd = other_sub_fd,   .events = POLLIN },
//		 */
//	};
//
//	int error_counter = 0;


	/*
	 * |-----------------------------------------------------|
	 * |                  INIT VARIABLES!                    |
	 * |-----------------------------------------------------|
	 */



	// inizializzazione middle-layer
	struct vehicle_attitude_s ahrs;
	struct unibo_joystick_s joystick;
	struct vehicle_local_position_s position;
	struct unibo_parameters_s param;

	static uint64_t time_pre = 0;
	static uint64_t nowT = 0;
	float deltaT;


	TRAJ_start();
	TRAJ_control();



	/* Bool for topics update */
	bool updated;

	float yawoffset = 0;   //offset between onboard yaw and optitrack yaw


	/*
	 * |-----------------------------------------------------|
	 * |                MAIN THREAD LOOP!                    |
	 * |-----------------------------------------------------|
	 */


	while (!thread_should_exit) {
		//TECNICAMENTE SIAMO GIA' A 50HZ, freq del topic di joystick (default 50HZ)

		nowT = hrt_absolute_time();
		deltaT = (nowT-time_pre)/1000.0f;
		if (deltaT>=20){              //50 Hz
			time_pre = nowT;
			//warnx("Trajectory APP: deltaT = %.2f",deltaT);    //milliseconds deltaT, should be 20

			/* copy raw JOYSTICK data into local buffer */
			orb_check(joystick_fd, &updated);
//.........这里部分代码省略.........

pthread_t
uorb_receive_start(void)
{
	/* --- SENSORS RAW VALUE --- */
	mavlink_subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
	/* rate limit set externally based on interface speed, set a basic default here */
	orb_set_interval(mavlink_subs.sensor_sub, 200);	/* 5Hz updates */

	/* --- ATTITUDE VALUE --- */
	mavlink_subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
	/* rate limit set externally based on interface speed, set a basic default here */
	orb_set_interval(mavlink_subs.att_sub, 200);	/* 5Hz updates */

	/* --- GPS VALUE --- */
	mavlink_subs.gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
	orb_set_interval(mavlink_subs.gps_sub, 200);	/* 5Hz updates */

	/* --- HOME POSITION --- */
	mavlink_subs.home_sub = orb_subscribe(ORB_ID(home_position));
	orb_set_interval(mavlink_subs.home_sub, 1000);	/* 1Hz updates */

	/* --- SYSTEM STATE --- */
	status_sub = orb_subscribe(ORB_ID(vehicle_status));
	orb_set_interval(status_sub, 300);		/* max 3.33 Hz updates */

	/* --- RC CHANNELS VALUE --- */
	rc_sub = orb_subscribe(ORB_ID(rc_channels));
	orb_set_interval(rc_sub, 100);			/* 10Hz updates */

	/* --- RC RAW VALUE --- */
	mavlink_subs.input_rc_sub = orb_subscribe(ORB_ID(input_rc));
	orb_set_interval(mavlink_subs.input_rc_sub, 100);

	/* --- GLOBAL POS VALUE --- */
	mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
	orb_set_interval(mavlink_subs.global_pos_sub, 100);	/* 10 Hz active updates */

	/* --- LOCAL POS VALUE --- */
	mavlink_subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
	orb_set_interval(mavlink_subs.local_pos_sub, 1000);	/* 1Hz active updates */

	/* --- GLOBAL SETPOINT VALUE --- */
	mavlink_subs.spg_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
	orb_set_interval(mavlink_subs.spg_sub, 2000);	/* 0.5 Hz updates */

	/* --- LOCAL SETPOINT VALUE --- */
	mavlink_subs.spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
	orb_set_interval(mavlink_subs.spl_sub, 2000);	/* 0.5 Hz updates */

	/* --- ATTITUDE SETPOINT VALUE --- */
	mavlink_subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
	orb_set_interval(mavlink_subs.spa_sub, 2000);	/* 0.5 Hz updates */

	/* --- RATES SETPOINT VALUE --- */
	mavlink_subs.rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
	orb_set_interval(mavlink_subs.rates_setpoint_sub, 2000);  /* 0.5 Hz updates */

	/* --- ACTUATOR OUTPUTS --- */
	mavlink_subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0));
	mavlink_subs.act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1));
	mavlink_subs.act_2_sub = orb_subscribe(ORB_ID(actuator_outputs_2));
	mavlink_subs.act_3_sub = orb_subscribe(ORB_ID(actuator_outputs_3));
	/* rate limits set externally based on interface speed, set a basic default here */
	orb_set_interval(mavlink_subs.act_0_sub, 100);	/* 10Hz updates */
	orb_set_interval(mavlink_subs.act_1_sub, 100);	/* 10Hz updates */
	orb_set_interval(mavlink_subs.act_2_sub, 100);	/* 10Hz updates */
	orb_set_interval(mavlink_subs.act_3_sub, 100);	/* 10Hz updates */

	/* --- ACTUATOR ARMED VALUE --- */
	mavlink_subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed));
	orb_set_interval(mavlink_subs.armed_sub, 100);	/* 10Hz updates */

	/* --- MAPPED MANUAL CONTROL INPUTS --- */
	mavlink_subs.man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
	/* rate limits set externally based on interface speed, set a basic default here */
	orb_set_interval(mavlink_subs.man_control_sp_sub, 100);	/* 10Hz updates */

	/* --- ACTUATOR CONTROL VALUE --- */
	mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
	orb_set_interval(mavlink_subs.actuators_sub, 100);	/* 10Hz updates */

	/* --- DEBUG VALUE OUTPUT --- */
	mavlink_subs.debug_key_value = orb_subscribe(ORB_ID(debug_key_value));
	orb_set_interval(mavlink_subs.debug_key_value, 100);	/* 10Hz updates */

	/* --- FLOW SENSOR --- */
	mavlink_subs.optical_flow = orb_subscribe(ORB_ID(optical_flow));
	orb_set_interval(mavlink_subs.optical_flow, 200); 	/* 5Hz updates */

	/* start the listener loop */
	pthread_attr_t uorb_attr;
	pthread_attr_init(&uorb_attr);

	/* Set stack size, needs less than 2k */
	pthread_attr_setstacksize(&uorb_attr, 2048);

	pthread_t thread;
	pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL);
	return thread;
}