/**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. * Author: Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ /** * @file orb_listener.c * Monitors ORB topics and sends update messages as appropriate. * * @author Lorenz Meier */ // XXX trim includes #include #include #include #include #include #include #include "mavlink_bridge_header.h" #include #include #include #include #include #include #include #include #include #include "waypoints.h" #include "orb_topics.h" #include "missionlib.h" #include "mavlink_hil.h" #include "util.h" extern bool gcs_link; struct vehicle_global_position_s global_pos; struct vehicle_local_position_s local_pos; struct navigation_capabilities_s nav_cap; struct vehicle_status_s v_status; struct rc_channels_s rc; struct rc_input_values rc_raw; struct actuator_armed_s armed; struct actuator_controls_s actuators_0; struct vehicle_attitude_s att; struct airspeed_s airspeed; struct mavlink_subscriptions mavlink_subs; static int status_sub; static int rc_sub; static unsigned int sensors_raw_counter; static unsigned int attitude_counter; static unsigned int gps_counter; /* * Last sensor loop time * some outputs are better timestamped * with this "global" reference. */ static uint64_t last_sensor_timestamp; static hrt_abstime last_sent_vfr = 0; static void *uorb_receive_thread(void *arg); struct listener { void (*callback)(const struct listener *l); int *subp; uintptr_t arg; }; uint16_t cm_uint16_from_m_float(float m); static void l_sensor_combined(const struct listener *l); static void l_vehicle_attitude(const struct listener *l); static void l_vehicle_gps_position(const struct listener *l); static void l_vehicle_status(const struct listener *l); static void l_rc_channels(const struct listener *l); static void l_input_rc(const struct listener *l); static void l_global_position(const struct listener *l); static void l_local_position(const struct listener *l); static void l_global_position_setpoint(const struct listener *l); static void l_local_position_setpoint(const struct listener *l); static void l_attitude_setpoint(const struct listener *l); static void l_actuator_outputs(const struct listener *l); static void l_actuator_armed(const struct listener *l); static void l_manual_control_setpoint(const struct listener *l); static void l_vehicle_attitude_controls(const struct listener *l); static void l_debug_key_value(const struct listener *l); static void l_optical_flow(const struct listener *l); static void l_vehicle_rates_setpoint(const struct listener *l); static void l_home(const struct listener *l); static void l_airspeed(const struct listener *l); static void l_nav_cap(const struct listener *l); static const struct listener listeners[] = { {l_sensor_combined, &mavlink_subs.sensor_sub, 0}, {l_vehicle_attitude, &mavlink_subs.att_sub, 0}, {l_vehicle_gps_position, &mavlink_subs.gps_sub, 0}, {l_vehicle_status, &status_sub, 0}, {l_rc_channels, &rc_sub, 0}, {l_input_rc, &mavlink_subs.input_rc_sub, 0}, {l_global_position, &mavlink_subs.global_pos_sub, 0}, {l_local_position, &mavlink_subs.local_pos_sub, 0}, {l_global_position_setpoint, &mavlink_subs.spg_sub, 0}, {l_local_position_setpoint, &mavlink_subs.spl_sub, 0}, {l_attitude_setpoint, &mavlink_subs.spa_sub, 0}, {l_actuator_outputs, &mavlink_subs.act_0_sub, 0}, {l_actuator_outputs, &mavlink_subs.act_1_sub, 1}, {l_actuator_outputs, &mavlink_subs.act_2_sub, 2}, {l_actuator_outputs, &mavlink_subs.act_3_sub, 3}, {l_actuator_armed, &mavlink_subs.armed_sub, 0}, {l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0}, {l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0}, {l_debug_key_value, &mavlink_subs.debug_key_value, 0}, {l_optical_flow, &mavlink_subs.optical_flow, 0}, {l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0}, {l_home, &mavlink_subs.home_sub, 0}, {l_airspeed, &mavlink_subs.airspeed_sub, 0}, {l_nav_cap, &mavlink_subs.navigation_capabilities_sub, 0}, }; static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]); uint16_t cm_uint16_from_m_float(float m) { if (m < 0.0f) { return 0; } else if (m > 655.35f) { return 65535; } return (uint16_t)(m * 100.0f); } void l_sensor_combined(const struct listener *l) { struct sensor_combined_s raw; /* copy sensors raw data into local buffer */ orb_copy(ORB_ID(sensor_combined), mavlink_subs.sensor_sub, &raw); last_sensor_timestamp = raw.timestamp; /* mark individual fields as changed */ uint16_t fields_updated = 0; static unsigned accel_counter = 0; static unsigned gyro_counter = 0; static unsigned mag_counter = 0; static unsigned baro_counter = 0; if (accel_counter != raw.accelerometer_counter) { /* mark first three dimensions as changed */ fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); accel_counter = raw.accelerometer_counter; } if (gyro_counter != raw.gyro_counter) { /* mark second group dimensions as changed */ fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); gyro_counter = raw.gyro_counter; } if (mag_counter != raw.magnetometer_counter) { /* mark third group dimensions as changed */ fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); mag_counter = raw.magnetometer_counter; } if (baro_counter != raw.baro_counter) { /* mark last group dimensions as changed */ fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); baro_counter = raw.baro_counter; } if (gcs_link) mavlink_msg_highres_imu_send(MAVLINK_COMM_0, last_sensor_timestamp, raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1], raw.accelerometer_m_s2[2], raw.gyro_rad_s[0], raw.gyro_rad_s[1], raw.gyro_rad_s[2], raw.magnetometer_ga[0], raw.magnetometer_ga[1], raw.magnetometer_ga[2], raw.baro_pres_mbar, raw.differential_pressure_pa, raw.baro_alt_meter, raw.baro_temp_celcius, fields_updated); sensors_raw_counter++; } void l_vehicle_attitude(const struct listener *l) { /* copy attitude data into local buffer */ orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att); if (gcs_link) { /* send sensor values */ mavlink_msg_attitude_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, att.roll, att.pitch, att.yaw, att.rollspeed, att.pitchspeed, att.yawspeed); /* limit VFR message rate to 10Hz */ hrt_abstime t = hrt_absolute_time(); if (t >= last_sent_vfr + 100000) { last_sent_vfr = t; float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; float throttle = armed.armed ? actuators_0.control[3] * 100.0f : 0.0f; mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz); } /* send quaternion values if it exists */ if(att.q_valid) { mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, att.q[0], att.q[1], att.q[2], att.q[3], att.rollspeed, att.pitchspeed, att.yawspeed); } } attitude_counter++; } void l_vehicle_gps_position(const struct listener *l) { struct vehicle_gps_position_s gps; /* copy gps data into local buffer */ orb_copy(ORB_ID(vehicle_gps_position), mavlink_subs.gps_sub, &gps); /* GPS COG is 0..2PI in degrees * 1e2 */ float cog_deg = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F; /* GPS position */ mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, gps.timestamp_position, gps.fix_type, gps.lat, gps.lon, gps.alt, cm_uint16_from_m_float(gps.eph_m), cm_uint16_from_m_float(gps.epv_m), gps.vel_m_s * 1e2f, // from m/s to cm/s cog_deg * 1e2f, // from deg to deg * 100 gps.satellites_visible); /* update SAT info every 10 seconds */ if (gps.satellite_info_available && (gps_counter % 50 == 0)) { mavlink_msg_gps_status_send(MAVLINK_COMM_0, gps.satellites_visible, gps.satellite_prn, gps.satellite_used, gps.satellite_elevation, gps.satellite_azimuth, gps.satellite_snr); } gps_counter++; } void l_vehicle_status(const struct listener *l) { /* immediately communicate state changes back to user */ orb_copy(ORB_ID(vehicle_status), status_sub, &v_status); orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); /* enable or disable HIL */ if (v_status.hil_state == HIL_STATE_ON) set_hil_on_off(true); else if (v_status.hil_state == HIL_STATE_OFF) set_hil_on_off(false); /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; uint8_t mavlink_base_mode = 0; uint32_t mavlink_custom_mode = 0; get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); /* send heartbeat */ mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, mavlink_custom_mode, mavlink_state); } void l_rc_channels(const struct listener *l) { /* copy rc channels into local buffer */ orb_copy(ORB_ID(rc_channels), rc_sub, &rc); // XXX Add RC channels scaled message here } void l_input_rc(const struct listener *l) { /* copy rc channels into local buffer */ orb_copy(ORB_ID(input_rc), mavlink_subs.input_rc_sub, &rc_raw); if (gcs_link) { const unsigned port_width = 8; for (unsigned i = 0; (i * port_width) < rc_raw.channel_count; i++) { /* Channels are sent in MAVLink main loop at a fixed interval */ mavlink_msg_rc_channels_raw_send(chan, rc_raw.timestamp_publication / 1000, i, (rc_raw.channel_count > (i * port_width) + 0) ? rc_raw.values[(i * port_width) + 0] : UINT16_MAX, (rc_raw.channel_count > (i * port_width) + 1) ? rc_raw.values[(i * port_width) + 1] : UINT16_MAX, (rc_raw.channel_count > (i * port_width) + 2) ? rc_raw.values[(i * port_width) + 2] : UINT16_MAX, (rc_raw.channel_count > (i * port_width) + 3) ? rc_raw.values[(i * port_width) + 3] : UINT16_MAX, (rc_raw.channel_count > (i * port_width) + 4) ? rc_raw.values[(i * port_width) + 4] : UINT16_MAX, (rc_raw.channel_count > (i * port_width) + 5) ? rc_raw.values[(i * port_width) + 5] : UINT16_MAX, (rc_raw.channel_count > (i * port_width) + 6) ? rc_raw.values[(i * port_width) + 6] : UINT16_MAX, (rc_raw.channel_count > (i * port_width) + 7) ? rc_raw.values[(i * port_width) + 7] : UINT16_MAX, rc_raw.rssi); } } } void l_global_position(const struct listener *l) { /* copy global position data into local buffer */ orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos); mavlink_msg_global_position_int_send(MAVLINK_COMM_0, global_pos.timestamp / 1000, global_pos.lat, global_pos.lon, global_pos.alt * 1000.0f, global_pos.relative_alt * 1000.0f, global_pos.vx * 100.0f, global_pos.vy * 100.0f, global_pos.vz * 100.0f, _wrap_2pi(global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f); } void l_local_position(const struct listener *l) { /* copy local position data into local buffer */ orb_copy(ORB_ID(vehicle_local_position), mavlink_subs.local_pos_sub, &local_pos); if (gcs_link) mavlink_msg_local_position_ned_send(MAVLINK_COMM_0, local_pos.timestamp / 1000, local_pos.x, local_pos.y, local_pos.z, local_pos.vx, local_pos.vy, local_pos.vz); } void l_global_position_setpoint(const struct listener *l) { struct vehicle_global_position_setpoint_s global_sp; /* copy local position data into local buffer */ orb_copy(ORB_ID(vehicle_global_position_setpoint), mavlink_subs.spg_sub, &global_sp); uint8_t coordinate_frame = MAV_FRAME_GLOBAL; if (global_sp.altitude_is_relative) coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; if (gcs_link) mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0, coordinate_frame, global_sp.lat, global_sp.lon, global_sp.altitude * 1000.0f, global_sp.yaw * M_RAD_TO_DEG_F * 100.0f); } void l_local_position_setpoint(const struct listener *l) { struct vehicle_local_position_setpoint_s local_sp; /* copy local position data into local buffer */ orb_copy(ORB_ID(vehicle_local_position_setpoint), mavlink_subs.spl_sub, &local_sp); if (gcs_link) mavlink_msg_local_position_setpoint_send(MAVLINK_COMM_0, MAV_FRAME_LOCAL_NED, local_sp.x, local_sp.y, local_sp.z, local_sp.yaw); } void l_attitude_setpoint(const struct listener *l) { struct vehicle_attitude_setpoint_s att_sp; /* copy local position data into local buffer */ orb_copy(ORB_ID(vehicle_attitude_setpoint), mavlink_subs.spa_sub, &att_sp); if (gcs_link) mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, att_sp.timestamp / 1000, att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, att_sp.thrust); } void l_vehicle_rates_setpoint(const struct listener *l) { struct vehicle_rates_setpoint_s rates_sp; /* copy local position data into local buffer */ orb_copy(ORB_ID(vehicle_rates_setpoint), mavlink_subs.rates_setpoint_sub, &rates_sp); if (gcs_link) mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(MAVLINK_COMM_0, rates_sp.timestamp / 1000, rates_sp.roll, rates_sp.pitch, rates_sp.yaw, rates_sp.thrust); } void l_actuator_outputs(const struct listener *l) { struct actuator_outputs_s act_outputs; orb_id_t ids[] = { ORB_ID(actuator_outputs_0), ORB_ID(actuator_outputs_1), ORB_ID(actuator_outputs_2), ORB_ID(actuator_outputs_3) }; /* copy actuator data into local buffer */ orb_copy(ids[l->arg], *l->subp, &act_outputs); if (gcs_link) { mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, l->arg /* port number - needs GCS support */, /* QGC has port number support already */ act_outputs.output[0], act_outputs.output[1], act_outputs.output[2], act_outputs.output[3], act_outputs.output[4], act_outputs.output[5], act_outputs.output[6], act_outputs.output[7]); /* only send in HIL mode and only send first group for HIL */ if (mavlink_hil_enabled && armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) { /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; uint8_t mavlink_base_mode = 0; uint32_t mavlink_custom_mode = 0; get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); /* HIL message as per MAVLink spec */ /* scale / assign outputs depending on system type */ if (mavlink_system.type == MAV_TYPE_QUADROTOR) { mavlink_msg_hil_controls_send(chan, hrt_absolute_time(), ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, -1, -1, -1, -1, mavlink_base_mode, 0); } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) { mavlink_msg_hil_controls_send(chan, hrt_absolute_time(), ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, -1, -1, mavlink_base_mode, 0); } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) { mavlink_msg_hil_controls_send(chan, hrt_absolute_time(), ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f, mavlink_base_mode, 0); } else { mavlink_msg_hil_controls_send(chan, hrt_absolute_time(), (act_outputs.output[0] - 1500.0f) / 500.0f, (act_outputs.output[1] - 1500.0f) / 500.0f, (act_outputs.output[2] - 1500.0f) / 500.0f, (act_outputs.output[3] - 1000.0f) / 1000.0f, (act_outputs.output[4] - 1500.0f) / 500.0f, (act_outputs.output[5] - 1500.0f) / 500.0f, (act_outputs.output[6] - 1500.0f) / 500.0f, (act_outputs.output[7] - 1500.0f) / 500.0f, mavlink_base_mode, 0); } } } } void l_actuator_armed(const struct listener *l) { orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); } void l_manual_control_setpoint(const struct listener *l) { struct manual_control_setpoint_s man_control; /* copy manual control data into local buffer */ orb_copy(ORB_ID(manual_control_setpoint), mavlink_subs.man_control_sp_sub, &man_control); if (gcs_link) mavlink_msg_manual_control_send(MAVLINK_COMM_0, mavlink_system.sysid, man_control.roll * 1000, man_control.pitch * 1000, man_control.yaw * 1000, man_control.throttle * 1000, 0); } void l_vehicle_attitude_controls(const struct listener *l) { orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators_0); if (gcs_link) { /* send, add spaces so that string buffer is at least 10 chars long */ mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl0 ", actuators_0.control[0]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl1 ", actuators_0.control[1]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl2 ", actuators_0.control[2]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl3 ", actuators_0.control[3]); } } void l_debug_key_value(const struct listener *l) { struct debug_key_value_s debug; orb_copy(ORB_ID(debug_key_value), mavlink_subs.debug_key_value, &debug); /* Enforce null termination */ debug.key[sizeof(debug.key) - 1] = '\0'; mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, debug.key, debug.value); } void l_optical_flow(const struct listener *l) { struct optical_flow_s flow; orb_copy(ORB_ID(optical_flow), mavlink_subs.optical_flow, &flow); mavlink_msg_optical_flow_send(MAVLINK_COMM_0, flow.timestamp, flow.sensor_id, flow.flow_raw_x, flow.flow_raw_y, flow.flow_comp_x_m, flow.flow_comp_y_m, flow.quality, flow.ground_distance_m); } void l_home(const struct listener *l) { struct home_position_s home; orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home); mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, home.lat, home.lon, home.alt); } void l_airspeed(const struct listener *l) { orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed); } void l_nav_cap(const struct listener *l) { orb_copy(ORB_ID(navigation_capabilities), mavlink_subs.navigation_capabilities_sub, &nav_cap); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "turn dist", nav_cap.turn_distance); } static void * uorb_receive_thread(void *arg) { /* Set thread name */ prctl(PR_SET_NAME, "mavlink_orb_rcv", getpid()); /* * set up poll to block for new data, * wait for a maximum of 1000 ms (1 second) */ const int timeout = 1000; /* * Initialise listener array. * * Might want to invoke each listener once to set initial state. */ struct pollfd fds[n_listeners]; for (unsigned i = 0; i < n_listeners; i++) { fds[i].fd = *listeners[i].subp; fds[i].events = POLLIN; /* Invoke callback to set initial state */ //listeners[i].callback(&listener[i]); } while (!thread_should_exit) { int poll_ret = poll(fds, n_listeners, timeout); /* handle the poll result */ if (poll_ret == 0) { /* silent */ } else if (poll_ret < 0) { mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data"); } else { for (unsigned i = 0; i < n_listeners; i++) { if (fds[i].revents & POLLIN) listeners[i].callback(&listeners[i]); } } } return NULL; } 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); 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 */ /* --- AIRSPEED --- */ mavlink_subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); orb_set_interval(mavlink_subs.airspeed_sub, 200); /* 5Hz updates */ /* --- NAVIGATION CAPABILITIES --- */ mavlink_subs.navigation_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); orb_set_interval(mavlink_subs.navigation_capabilities_sub, 500); /* 2Hz updates */ nav_cap.turn_distance = 0.0f; /* 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); pthread_attr_destroy(&uorb_attr); return thread; }