C++ orb_set_interval函数代码示例

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 },
		 */
	};