本文整理汇总了C++中orb_check函数的典型用法代码示例。如果您正苦于以下问题:C++ orb_check函数的具体用法?C++ orb_check怎么用?C++ orb_check使用的例子?那么恭喜您, 这里精选的函数代码示例或许可以为您提供帮助。
在下文中一共展示了orb_check函数的15个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: orb_check
void estimator_base::gps_poll()
{
_gps_new = false;
orb_check(_gps_sub, &_gps_new);
if (_gps_new) {
orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps);
}
if (_gps.fix_type < 3 || !isfinite(_gps.lon) || !isfinite(_gps.lat) || !isfinite(_gps.alt) ) {
_gps_new = false;
}
if (_gps_new && !_gps_init) {
_gps_init = true;
_init_lon = _gps.lon;
_init_lat = _gps.lat;
_init_alt = _gps.alt;
}
}
示例2: orb_check
void
MulticopterAttitudeControl::vehicle_status_poll()
{
/* check if there is new status information */
bool vehicle_status_updated;
orb_check(_vehicle_status_sub, &vehicle_status_updated);
if (vehicle_status_updated) {
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
/* set correct uORB ID, depending on if vehicle is VTOL or not */
if (!_rates_sp_id) {
if (_vehicle_status.is_vtol) {
_rates_sp_id = ORB_ID(mc_virtual_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_virtual_mc);
} else {
_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_0);
}
}
}
}
示例3: orb_check
void
FixedwingPositionControl::vehicle_control_mode_poll()
{
bool vstatus_updated;
/* Check HIL state if vehicle status has changed */
orb_check(_control_mode_sub, &vstatus_updated);
if (vstatus_updated) {
bool was_armed = _control_mode.flag_armed;
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
if (!was_armed && _control_mode.flag_armed) {
_launch_lat = _global_pos.lat;
_launch_lon = _global_pos.lon;
_launch_alt = _global_pos.alt;
_launch_valid = true;
}
}
}
示例4: orb_check
void MulticopterLandDetector::updateParameterCache(const bool force)
{
bool updated;
parameter_update_s paramUpdate;
orb_check(_parameterSub, &updated);
if (updated) {
orb_copy(ORB_ID(parameter_update), _parameterSub, ¶mUpdate);
}
if (updated || force) {
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.maxThrottle, &_params.maxThrottle);
param_get(_paramHandle.minManThrottle, &_params.minManThrottle);
param_get(_paramHandle.acc_threshold_m_s2, &_params.acc_threshold_m_s2);
param_get(_paramHandle.ff_trigger_time, &_params.ff_trigger_time);
}
}
示例5: orb_check
void OutputBase::_handle_position_update(bool force_update)
{
bool need_update = force_update;
if (!_cur_control_data || _cur_control_data->type != ControlData::Type::LonLat) {
return;
}
if (!force_update) {
orb_check(_vehicle_global_position_sub, &need_update);
}
if (!need_update) {
return;
}
vehicle_global_position_s vehicle_global_position;
orb_copy(ORB_ID(vehicle_global_position), _vehicle_global_position_sub, &vehicle_global_position);
float pitch;
const double &lon = _cur_control_data->type_data.lonlat.lon;
const double &lat = _cur_control_data->type_data.lonlat.lat;
const float &alt = _cur_control_data->type_data.lonlat.altitude;
if (_cur_control_data->type_data.lonlat.pitch_fixed_angle >= -M_PI_F) {
pitch = _cur_control_data->type_data.lonlat.pitch_fixed_angle;
} else {
pitch = _calculate_pitch(lon, lat, alt, vehicle_global_position);
}
float roll = _cur_control_data->type_data.lonlat.roll_angle;
float yaw = get_bearing_to_next_waypoint(vehicle_global_position.lat, vehicle_global_position.lon, lat, lon)
- vehicle_global_position.yaw;
_angle_setpoints[0] = roll;
_angle_setpoints[1] = pitch + _cur_control_data->type_data.lonlat.pitch_angle_offset;
_angle_setpoints[2] = yaw + _cur_control_data->type_data.lonlat.yaw_angle_offset;
}
示例6: hrt_absolute_time
bool
MavlinkOrbSubscription::is_published()
{
// If we marked it as published no need to check again
if (_published) {
return true;
}
hrt_abstime now = hrt_absolute_time();
if (now - _last_pub_check < 300000) {
return false;
}
// We are checking now
_last_pub_check = now;
// We don't want to subscribe to anything that does not exist
// in order to save memory and file descriptors.
// However, for some topics like vehicle_command_ack, we want to subscribe
// from the beginning in order not to miss or delay the first publish respective advertise.
if (!_subscribe_from_beginning && orb_exists(_topic, _instance)) {
return false;
}
if (_fd < 0) {
_fd = orb_subscribe_multi(_topic, _instance);
}
bool updated;
orb_check(_fd, &updated);
if (updated) {
_published = true;
}
return _published;
}
示例7: update
bool
MavlinkOrbSubscription::update_if_changed(void *data)
{
bool prevpub = _published;
if (!is_published()) {
return false;
}
bool updated;
if (orb_check(_fd, &updated)) {
return false;
}
// If we didn't update and this topic did not change
// its publication status then nothing really changed
if (!updated && prevpub == _published) {
return false;
}
return update(data);
}
示例8: orb_check
void GPS::handleInjectDataTopic()
{
if (_orb_inject_data_fd[0] == -1) {
return;
}
bool updated = false;
do {
int orb_inject_data_cur_fd = _orb_inject_data_fd[_orb_inject_data_next];
orb_check(orb_inject_data_cur_fd, &updated);
if (updated) {
struct gps_inject_data_s msg;
orb_copy(ORB_ID(gps_inject_data), orb_inject_data_cur_fd, &msg);
injectData(msg.data, msg.len);
_orb_inject_data_next = (_orb_inject_data_next + 1) % _orb_inject_data_fd_count;
++_last_rate_rtcm_injection_count;
}
} while (updated);
}
示例9: do_trim_calibration
int do_trim_calibration(int mavlink_fd)
{
int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
usleep(200000);
struct manual_control_setpoint_s sp;
bool changed;
orb_check(sub_man, &changed);
if (!changed) {
mavlink_log_critical(mavlink_fd, "no inputs, aborting");
return ERROR;
}
orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);
/* set parameters */
float p = sp.roll;
param_set(param_find("TRIM_ROLL"), &p);
p = sp.pitch;
param_set(param_find("TRIM_PITCH"), &p);
p = sp.yaw;
param_set(param_find("TRIM_YAW"), &p);
/* store to permanent storage */
/* auto-save */
int save_ret = param_save_default();
if (save_ret != 0) {
mavlink_log_critical(mavlink_fd, "TRIM: SAVE FAIL");
close(sub_man);
return ERROR;
}
mavlink_log_info(mavlink_fd, "trim cal done");
close(sub_man);
return OK;
}
示例10: orb_check
void
MulticopterPositionControl::poll_subscriptions()
{
bool updated;
orb_check(_att_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
}
orb_check(_att_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp);
}
orb_check(_control_mode_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
}
orb_check(_manual_sub, &updated);
if (updated) {
orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual);
}
orb_check(_arming_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming);
}
orb_check(_local_pos_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
}
}
示例11: orb_subscribe
void Ekf2::task_main()
{
// subscribe to relevant topics
int sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
int airspeed_sub = orb_subscribe(ORB_ID(airspeed));
int params_sub = orb_subscribe(ORB_ID(parameter_update));
int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
int range_finder_sub = orb_subscribe(ORB_ID(distance_sensor));
int ev_pos_sub = orb_subscribe(ORB_ID(vehicle_vision_position));
int ev_att_sub = orb_subscribe(ORB_ID(vehicle_vision_attitude));
int vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
int status_sub = orb_subscribe(ORB_ID(vehicle_status));
px4_pollfd_struct_t fds[2] = {};
fds[0].fd = sensors_sub;
fds[0].events = POLLIN;
fds[1].fd = params_sub;
fds[1].events = POLLIN;
// initialise parameter cache
updateParams();
// initialize data structures outside of loop
// because they will else not always be
// properly populated
sensor_combined_s sensors = {};
vehicle_gps_position_s gps = {};
airspeed_s airspeed = {};
optical_flow_s optical_flow = {};
distance_sensor_s range_finder = {};
vehicle_land_detected_s vehicle_land_detected = {};
vehicle_local_position_s ev_pos = {};
vehicle_attitude_s ev_att = {};
vehicle_status_s vehicle_status = {};
while (!_task_should_exit) {
int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000);
if (ret < 0) {
// Poll error, sleep and try again
usleep(10000);
continue;
} else if (ret == 0) {
// Poll timeout or no new data, do nothing
continue;
}
if (fds[1].revents & POLLIN) {
// read from param to clear updated flag
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), params_sub, &update);
updateParams();
// fetch sensor data in next loop
continue;
} else if (!(fds[0].revents & POLLIN)) {
// no new data
continue;
}
bool gps_updated = false;
bool airspeed_updated = false;
bool optical_flow_updated = false;
bool range_finder_updated = false;
bool vehicle_land_detected_updated = false;
bool vision_position_updated = false;
bool vision_attitude_updated = false;
bool vehicle_status_updated = false;
orb_copy(ORB_ID(sensor_combined), sensors_sub, &sensors);
// update all other topics if they have new data
orb_check(status_sub, &vehicle_status_updated);
if (vehicle_status_updated) {
orb_copy(ORB_ID(vehicle_status), status_sub, &vehicle_status);
}
orb_check(gps_sub, &gps_updated);
if (gps_updated) {
orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps);
}
orb_check(airspeed_sub, &airspeed_updated);
if (airspeed_updated) {
orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed);
}
orb_check(optical_flow_sub, &optical_flow_updated);
if (optical_flow_updated) {
orb_copy(ORB_ID(optical_flow), optical_flow_sub, &optical_flow);
}
orb_check(range_finder_sub, &range_finder_updated);
//.........这里部分代码省略.........
示例12: position_estimator_inav_thread_main
//.........这里部分代码省略.........
mavlink_log_info(mavlink_fd, "[inav] baro offset: %d m", (int)baro_offset);
local_pos.z_valid = true;
local_pos.v_z_valid = true;
}
}
}
}
}
/* main loop */
px4_pollfd_struct_t fds[1] = {
{ .fd = vehicle_attitude_sub, .events = POLLIN },
};
while (!thread_should_exit) {
int ret = px4_poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate
hrt_abstime t = hrt_absolute_time();
if (ret < 0) {
/* poll error */
mavlink_log_info(mavlink_fd, "[inav] poll error on init");
continue;
} else if (ret > 0) {
/* act on attitude updates */
/* vehicle attitude */
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
attitude_updates++;
bool updated;
/* parameter update */
orb_check(parameter_update_sub, &updated);
if (updated) {
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
inav_parameters_update(&pos_inav_param_handles, ¶ms);
}
/* actuator */
orb_check(actuator_sub, &updated);
if (updated) {
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator);
}
/* armed */
orb_check(armed_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
}
/* sensor combined */
orb_check(sensor_combined_sub, &updated);
if (updated) {
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
if (sensor.accelerometer_timestamp != accel_timestamp) {
if (att.R_valid) {
/* correct accel bias */
sensor.accelerometer_m_s2[0] -= acc_bias[0];
sensor.accelerometer_m_s2[1] -= acc_bias[1];
示例13: attitude_estimator_ekf_thread_main
//.........这里部分代码省略.........
fds[1].fd = sub_params;
fds[1].events = POLLIN;
int ret = px4_poll(fds, 2, 1000);
if (ret < 0) {
/* XXX this is seriously bad - should be an emergency */
} else if (ret == 0) {
/* check if we're in HIL - not getting sensor data is fine then */
orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode);
if (!control_mode.flag_system_hil_enabled) {
warnx("WARNING: Not getting sensor data - sensor app running?");
}
} else {
/* only update parameters if they changed */
if (fds[1].revents & POLLIN) {
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), sub_params, &update);
/* update parameters */
parameters_update(&ekf_param_handles, &ekf_params);
}
/* only run filter if sensor values changed */
if (fds[0].revents & POLLIN) {
/* get latest measurements */
orb_copy(ORB_ID(sensor_combined), sub_raw, &raw);
bool gps_updated;
orb_check(sub_gps, &gps_updated);
if (gps_updated) {
orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps);
if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp) < 1000000) {
mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f));
/* update mag declination rotation matrix */
R_decl.from_euler(0.0f, 0.0f, mag_decl);
}
}
bool global_pos_updated;
orb_check(sub_global_pos, &global_pos_updated);
if (global_pos_updated) {
orb_copy(ORB_ID(vehicle_global_position), sub_global_pos, &global_pos);
}
if (!initialized) {
// XXX disabling init for now
initialized = true;
// gyro_offsets[0] += raw.gyro_rad_s[0];
// gyro_offsets[1] += raw.gyro_rad_s[1];
// gyro_offsets[2] += raw.gyro_rad_s[2];
// offset_count++;
// if (hrt_absolute_time() - start_time > 3000000LL) {
// initialized = true;
// gyro_offsets[0] /= offset_count;
// gyro_offsets[1] /= offset_count;
// gyro_offsets[2] /= offset_count;
// }
示例14: mavlink_log_critical
void
Delivery::to_destination()
{
// set a mission to destination with takeoff enabled
// Status = enroute ; change to Dropoff at completion
if (_complete) {
// Update status now that travel to destination is complete and reset _first_run for next stage
_first_run = true;
mavlink_log_critical(_navigator->get_mavlink_fd(), "Black Hawk at Location");
advance_delivery();
return;
}
check_mission_valid();
/* check if anything has changed */
bool onboard_updated = false;
orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
if (onboard_updated) {
update_onboard_mission();
}
bool offboard_updated = false;
orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);
if (offboard_updated) {
update_offboard_mission();
}
/* reset mission items if needed */
if (onboard_updated || offboard_updated) {
set_mission_items();
}
/* lets check if we reached the current mission item */
if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
set_mission_item_reached();
if (_mission_item.autocontinue) {
/* switch to next waypoint if 'autocontinue' flag set */
advance_mission();
set_mission_items();
mavlink_log_critical(_navigator->get_mavlink_fd(), "Black Hawk advancing mission");
}
} else if (_mission_type != MISSION_TYPE_NONE &&_param_altmode.get() == MISSION_ALTMODE_FOH) {
altitude_sp_foh_update();
} else {
/* if waypoint position reached allow loiter on the setpoint */
if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) {
_navigator->set_can_loiter_at_sp(true);
_complete = true;
}
}
/* see if we need to update the current yaw heading for rotary wing types */
if (_navigator->get_vstatus()->is_rotary_wing
&& _param_yawmode.get() != MISSION_YAWMODE_NONE
&& _mission_type != MISSION_TYPE_NONE) {
heading_sp_update();
}
}
示例15: orb_subscribe
void
BlinkM::led()
{
static int vehicle_status_sub_fd;
static int vehicle_gps_position_sub_fd;
static int num_of_cells = 0;
static int detected_cells_runcount = 0;
static int t_led_color[8] = { 0, 0, 0, 0, 0, 0, 0, 0};
static int t_led_blink = 0;
static int led_thread_runcount=0;
static int led_interval = 1000;
static int no_data_vehicle_status = 0;
static int no_data_vehicle_gps_position = 0;
static bool topic_initialized = false;
static bool detected_cells_blinked = false;
static bool led_thread_ready = true;
int num_of_used_sats = 0;
if(!topic_initialized) {
vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
orb_set_interval(vehicle_status_sub_fd, 1000);
vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position));
orb_set_interval(vehicle_gps_position_sub_fd, 1000);
topic_initialized = true;
}
if(led_thread_ready == true) {
if(!detected_cells_blinked) {
if(num_of_cells > 0) {
t_led_color[0] = LED_PURPLE;
}
if(num_of_cells > 1) {
t_led_color[1] = LED_PURPLE;
}
if(num_of_cells > 2) {
t_led_color[2] = LED_PURPLE;
}
if(num_of_cells > 3) {
t_led_color[3] = LED_PURPLE;
}
if(num_of_cells > 4) {
t_led_color[4] = LED_PURPLE;
}
t_led_color[5] = LED_OFF;
t_led_color[6] = LED_OFF;
t_led_color[7] = LED_OFF;
t_led_blink = LED_BLINK;
} else {
t_led_color[0] = led_color_1;
t_led_color[1] = led_color_2;
t_led_color[2] = led_color_3;
t_led_color[3] = led_color_4;
t_led_color[4] = led_color_5;
t_led_color[5] = led_color_6;
t_led_color[6] = led_color_7;
t_led_color[7] = led_color_8;
t_led_blink = led_blink;
}
led_thread_ready = false;
}
if (led_thread_runcount & 1) {
if (t_led_blink)
setLEDColor(LED_OFF);
led_interval = LED_OFFTIME;
} else {
setLEDColor(t_led_color[(led_thread_runcount / 2) % 8]);
//led_interval = (led_thread_runcount & 1) : LED_ONTIME;
led_interval = LED_ONTIME;
}
if (led_thread_runcount == 15) {
/* obtained data for the first file descriptor */
struct vehicle_status_s vehicle_status_raw;
struct vehicle_gps_position_s vehicle_gps_position_raw;
memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw));
memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw));
bool new_data_vehicle_status;
bool new_data_vehicle_gps_position;
orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);
if (new_data_vehicle_status) {
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw);
no_data_vehicle_status = 0;
} else {
no_data_vehicle_status++;
if(no_data_vehicle_status >= 3)
no_data_vehicle_status = 3;
}
//.........这里部分代码省略.........