From 1e3d2acbf66b1101a9b17f97d2b786ffaa0e423a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 28 Jan 2014 19:30:23 +0100 Subject: Not building yet, things are coming together slowly on mavlink app --- src/modules/mavlink/mavlink_main.cpp | 18 +- src/modules/mavlink/mavlink_main.h | 50 ++- src/modules/mavlink/mavlink_orb_listener.cpp | 557 ++++++++++++++------------- src/modules/mavlink/mavlink_orb_listener.h | 94 +++-- src/modules/mavlink/mavlink_receiver.cpp | 24 +- src/modules/mavlink/mavlink_receiver.h | 4 +- 6 files changed, 426 insertions(+), 321 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1c7986cbb..cd37c5437 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -56,7 +56,6 @@ #include #include #include -#include #include #include #include @@ -196,6 +195,11 @@ Mavlink::~Mavlink() } } +void Mavlink::set_mode(enum MAVLINK_MODE mode) +{ + _mode = mode; +} + int Mavlink::instance_count() { /* note: a local buffer count will help if this ever is called often */ @@ -1506,7 +1510,7 @@ Mavlink::task_main(int argc, char *argv[]) break; case 'o': - mavlink_link_mode = MAVLINK_INTERFACE_MODE_ONBOARD; + _mode = MODE_ONBOARD; break; default: @@ -1523,7 +1527,7 @@ Mavlink::task_main(int argc, char *argv[]) warnx("MAVLink v1.0 serial interface starting..."); /* inform about mode */ - warnx((mavlink_link_mode == MAVLINK_INTERFACE_MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE"); + warnx((_mode == MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE"); /* Flush stdout in case MAVLink is about to take it over */ fflush(stdout); @@ -1541,12 +1545,12 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_update_system(); /* start the MAVLink receiver */ - MavlinkReceiver rcv(this); - receive_thread = rcv.receive_start(uart); + // MavlinkReceiver rcv(this); + receive_thread = MavlinkReceiver::receive_start(this); /* start the ORB receiver */ - MavlinkOrbListener listener(this); - uorb_receive_thread = listener.uorb_receive_start(); + //MavlinkOrbListener listener(this); + uorb_receive_thread = MavlinkOrbListener::uorb_receive_start(this); /* initialize waypoint manager */ mavlink_wpm_init(wpm); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 244af04a6..3b6714559 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -54,9 +54,7 @@ #include #include #include -#include #include -#include #include #include #include @@ -72,7 +70,8 @@ #include #include #include - +#include +#include #include @@ -139,6 +138,7 @@ struct mavlink_wpm_storage { class Mavlink { + public: /** * Constructor @@ -170,6 +170,31 @@ public: static int get_uart_fd(unsigned index); + int get_uart_fd() { return _mavlink_fd; } + + enum MAVLINK_MODE { + MODE_TX_HEARTBEAT_ONLY=0, + MODE_OFFBOARD, + MODE_ONBOARD, + MODE_HIL + }; + + void set_mode(enum MAVLINK_MODE); + enum MAVLINK_MODE get_mode(); + + bool hil_enabled() { return _mavlink_hil_enabled; }; + + /** + * Handle waypoint related messages. + */ + void mavlink_wpm_message_handler(const mavlink_message_t *msg); + + /** + * Handle parameter related messages. + */ + void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); + + struct mavlink_subscriptions { int sensor_sub; int att_sub; @@ -196,10 +221,15 @@ public: int airspeed_sub; int navigation_capabilities_sub; int control_mode_sub; + int rc_sub; + int status_sub; }; struct mavlink_subscriptions subs; + struct mavlink_subscriptions* get_subs() { return &subs; } + mavlink_channel_t get_chan() { return chan; } + /** Global position */ struct vehicle_global_position_s global_pos; /** Local position */ @@ -243,6 +273,7 @@ private: uint8_t missionlib_msg_buf[300]; //XXX MAGIC NUMBER bool thread_running; bool thread_should_exit; + MAVLINK_MODE _mode; uint8_t mavlink_wpm_comp_id; mavlink_channel_t chan; @@ -269,12 +300,6 @@ private: */ unsigned int mavlink_param_queue_index; - /* interface mode */ - enum { - MAVLINK_INTERFACE_MODE_OFFBOARD, - MAVLINK_INTERFACE_MODE_ONBOARD - } mavlink_link_mode; - struct mavlink_logbuffer lb; bool mavlink_link_termination_allowed; @@ -293,12 +318,6 @@ private: */ int set_hil_on_off(bool hil_enabled); - - /** - * Handle parameter related messages. - */ - void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); - /** * Send all parameters at once. * @@ -352,7 +371,6 @@ private: void mavlink_update_system(); - void mavlink_wpm_message_handler(const mavlink_message_t *msg); void mavlink_waypoint_eventloop(uint64_t now); void mavlink_wpm_send_waypoint_reached(uint16_t seq); void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq); diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp index 44bf77bb0..fa1a6887e 100644 --- a/src/modules/mavlink/mavlink_orb_listener.cpp +++ b/src/modules/mavlink/mavlink_orb_listener.cpp @@ -62,11 +62,7 @@ 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); @@ -86,86 +82,97 @@ cm_uint16_from_m_float(float m) MavlinkOrbListener::MavlinkOrbListener(Mavlink* parent) : - _task_should_exit(false), + thread_should_exit(false), _loop_perf(perf_alloc(PC_ELAPSED, "mavlink orb")), _mavlink(parent), + _listeners(nullptr), _n_listeners(0) { - 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.triplet_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}, - {l_control_mode, &mavlink_subs.control_mode_sub, 0}, -}; - - _n_listeners = sizeof(listeners) / sizeof(listeners[0]); + add_listener(MavlinkOrbListener::l_sensor_combined, &_mavlink->get_subs()->sensor_sub, 0); + add_listener(MavlinkOrbListener::l_vehicle_attitude, &_mavlink->get_subs()->att_sub, 0); + add_listener(MavlinkOrbListener::l_vehicle_gps_position, &_mavlink->get_subs()->gps_sub, 0); + add_listener(MavlinkOrbListener::l_vehicle_status, &status_sub, 0); + add_listener(MavlinkOrbListener::l_rc_channels, &rc_sub, 0); + add_listener(MavlinkOrbListener::l_input_rc, &_mavlink->get_subs()->input_rc_sub, 0); + add_listener(MavlinkOrbListener::l_global_position, &_mavlink->get_subs()->global_pos_sub, 0); + add_listener(MavlinkOrbListener::l_local_position, &_mavlink->get_subs()->local_pos_sub, 0); + add_listener(MavlinkOrbListener::l_global_position_setpoint, &_mavlink->get_subs()->triplet_sub, 0); + add_listener(MavlinkOrbListener::l_local_position_setpoint, &_mavlink->get_subs()->spl_sub, 0); + add_listener(MavlinkOrbListener::l_attitude_setpoint, &_mavlink->get_subs()->spa_sub, 0); + add_listener(MavlinkOrbListener::l_actuator_outputs, &_mavlink->get_subs()->act_0_sub, 0); + add_listener(MavlinkOrbListener::l_actuator_outputs, &_mavlink->get_subs()->act_1_sub, 1); + add_listener(MavlinkOrbListener::l_actuator_outputs, &_mavlink->get_subs()->act_2_sub, 2); + add_listener(MavlinkOrbListener::l_actuator_outputs, &_mavlink->get_subs()->act_3_sub, 3); + add_listener(MavlinkOrbListener::l_actuator_armed, &_mavlink->get_subs()->armed_sub, 0); + add_listener(MavlinkOrbListener::l_manual_control_setpoint, &_mavlink->get_subs()->man_control_sp_sub, 0); + add_listener(MavlinkOrbListener::l_vehicle_attitude_controls, &_mavlink->get_subs()->actuators_sub, 0); + add_listener(MavlinkOrbListener::l_debug_key_value, &_mavlink->get_subs()->debug_key_value, 0); + add_listener(MavlinkOrbListener::l_optical_flow, &_mavlink->get_subs()->optical_flow, 0); + add_listener(MavlinkOrbListener::l_vehicle_rates_setpoint, &_mavlink->get_subs()->rates_setpoint_sub, 0); + add_listener(MavlinkOrbListener::l_home, &_mavlink->get_subs()->home_sub, 0); + add_listener(MavlinkOrbListener::l_airspeed, &_mavlink->get_subs()->airspeed_sub, 0); + add_listener(MavlinkOrbListener::l_nav_cap, &_mavlink->get_subs()->navigation_capabilities_sub, 0); + add_listener(MavlinkOrbListener::l_control_mode, &_mavlink->get_subs()->control_mode_sub, 0); } +void MavlinkOrbListener::add_listener(void (*callback)(const struct listener *l), int *subp, uintptr_t arg) +{ + struct listener *nl = new listener; + + nl->callback = callback; + nl->subp = subp; + nl->arg = arg; + nl->next = nullptr; + + // Register it + struct listener *next = _listeners; + while (next->next != nullptr) { + next = next->next; + } + + // Attach + next->next = nl; + _n_listeners++; +} + void MavlinkOrbListener::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; + orb_copy(ORB_ID(sensor_combined), l->mavlink->get_subs()->sensor_sub, &raw); - /* mark individual fields as changed */ + /* mark individual fields as _mavlink->get_chan()ged */ 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->get_mavlink_chan(), last_sensor_timestamp, + // if (accel_counter != raw.accelerometer_counter) { + // /* mark first three dimensions as _mavlink->get_chan()ged */ + // fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); + // accel_counter = raw.accelerometer_counter; + // } + + // if (gyro_counter != raw.gyro_counter) { + // /* mark second group dimensions as _mavlink->get_chan()ged */ + // fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); + // gyro_counter = raw.gyro_counter; + // } + + // if (mag_counter != raw.magnetometer_counter) { + // /* mark third group dimensions as _mavlink->get_chan()ged */ + // fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); + // mag_counter = raw.magnetometer_counter; + // } + + // if (baro_counter != raw.baro_counter) { + // /* mark last group dimensions as _mavlink->get_chan()ged */ + // fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); + // baro_counter = raw.baro_counter; + // } + + if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) + mavlink_msg_highres_imu_send(l->mavlink->get_chan(), l->listener->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], @@ -175,51 +182,51 @@ MavlinkOrbListener::l_sensor_combined(const struct listener *l) raw.baro_alt_meter, raw.baro_temp_celcius, fields_updated); - sensors_raw_counter++; + l->listener->sensors_raw_counter++; } void MavlinkOrbListener::l_vehicle_attitude(const struct listener *l) { /* copy attitude data into local buffer */ - orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att); + orb_copy(ORB_ID(vehicle_attitude), l->mavlink->get_subs()->att_sub, &l->listener->att); - if (gcs_link) { + if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { /* send sensor values */ - mavlink_msg_attitude_send(_mavlink->get_mavlink_chan(), - last_sensor_timestamp / 1000, - att.roll, - att.pitch, - att.yaw, - att.rollspeed, - att.pitchspeed, - att.yawspeed); + mavlink_msg_attitude_send(l->mavlink->get_chan(), + l->listener->last_sensor_timestamp / 1000, + l->listener->att.roll, + l->listener->att.pitch, + l->listener->att.yaw, + l->listener->att.rollspeed, + l->listener->att.pitchspeed, + l->listener->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.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e); - 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->get_mavlink_chan(), airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vel_d); + if (t >= l->listener->last_sent_vfr + 100000) { + l->listener->last_sent_vfr = t; + float groundspeed = sqrtf(l->listener->global_pos.vel_n * l->listener->global_pos.vel_n + l->listener->global_pos.vel_e * l->listener->global_pos.vel_e); + uint16_t heading = _wrap_2pi(l->listener->att.yaw) * M_RAD_TO_DEG_F; + float throttle = l->listener->armed.armed ? l->listener->actuators_0.control[3] * 100.0f : 0.0f; + mavlink_msg_vfr_hud_send(l->mavlink->get_chan(), l->listener->airspeed.true_airspeed_m_s, groundspeed, heading, throttle, l->listener->global_pos.alt, -l->listener->global_pos.vel_d); } /* send quaternion values if it exists */ - if(att.q_valid) { - mavlink_msg_attitude_quaternion_send(_mavlink->get_mavlink_chan(), - last_sensor_timestamp / 1000, - att.q[0], - att.q[1], - att.q[2], - att.q[3], - att.rollspeed, - att.pitchspeed, - att.yawspeed); + if(l->listener->att.q_valid) { + mavlink_msg_attitude_quaternion_send(l->mavlink->get_chan(), + l->listener->last_sensor_timestamp / 1000, + l->listener->att.q[0], + l->listener->att.q[1], + l->listener->att.q[2], + l->listener->att.q[3], + l->listener->att.rollspeed, + l->listener->att.pitchspeed, + l->listener->att.yawspeed); } } - attitude_counter++; + l->listener->attitude_counter++; } void @@ -228,13 +235,13 @@ MavlinkOrbListener::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); + orb_copy(ORB_ID(vehicle_gps_position), l->mavlink->get_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->get_mavlink_chan(), + mavlink_msg_gps_raw_int_send(l->mavlink->get_chan(), gps.timestamp_position, gps.fix_type, gps.lat, @@ -247,8 +254,8 @@ MavlinkOrbListener::l_vehicle_gps_position(const struct listener *l) gps.satellites_visible); /* update SAT info every 10 seconds */ - if (gps.satellite_info_available && (gps_counter % 50 == 0)) { - mavlink_msg_gps_status_send(_mavlink->get_mavlink_chan(), + if (gps.satellite_info_available && (l->listener->gps_counter % 50 == 0)) { + mavlink_msg_gps_status_send(l->mavlink->get_chan(), gps.satellites_visible, gps.satellite_prn, gps.satellite_used, @@ -257,30 +264,30 @@ MavlinkOrbListener::l_vehicle_gps_position(const struct listener *l) gps.satellite_snr); } - gps_counter++; + l->listener->gps_counter++; } void MavlinkOrbListener::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); + /* immediately communicate state _mavlink->get_chan()ges back to user */ + orb_copy(ORB_ID(vehicle_status), l->listener->status_sub, &l->listener->v_status); + orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->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); + if (l->listener->v_status.hil_state == HIL_STATE_ON) + l->mavlink->set_hil_on_off(true); + else if (l->listener->v_status.hil_state == HIL_STATE_OFF) + l->mavlink->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); + l->mavlink->get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, + mavlink_msg_heartbeat_send(l->mavlink->get_chan(), mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, @@ -289,37 +296,37 @@ MavlinkOrbListener::l_vehicle_status(const struct listener *l) } void -MavlinkOrbListener::l_rc_channels(const struct listener *l) +MavlinkOrbListener::l_rc__mavlink->get_chan()nels(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 + /* copy rc _mavlink->get_chan()nels into local buffer */ + orb_copy(ORB_ID(rc__mavlink->get_chan()nels), rc_sub, &rc); + // XXX Add RC _mavlink->get_chan()nels scaled message here } void MavlinkOrbListener::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); + /* copy rc _mavlink->get_chan()nels into local buffer */ + orb_copy(ORB_ID(input_rc), l->mavlink->get_subs()->input_rc_sub, &l->listener->rc_raw); - if (gcs_link) { + if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { const unsigned port_width = 8; - for (unsigned i = 0; (i * port_width) < (rc_raw.channel_count + port_width); i++) { + for (unsigned i = 0; (i * port_width) < (l->listener->rc_raw.channel_count + port_width); i++) { /* Channels are sent in MAVLink main loop at a fixed interval */ - mavlink_msg_rc_channels_raw_send(chan, - rc_raw.timestamp / 1000, + mavlink_msg_rc_channels_raw_send(_mavlink->get_chan(), + l->listener->rc_raw.timestamp / 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); + (l->listener->rc_raw.channel_count > (i * port_width) + 0) ? l->listener->rc_raw.values[(i * port_width) + 0] : UINT16_MAX, + (l->listener->rc_raw.channel_count > (i * port_width) + 1) ? l->listener->rc_raw.values[(i * port_width) + 1] : UINT16_MAX, + (l->listener->rc_raw.channel_count > (i * port_width) + 2) ? l->listener->rc_raw.values[(i * port_width) + 2] : UINT16_MAX, + (l->listener->rc_raw.channel_count > (i * port_width) + 3) ? l->listener->rc_raw.values[(i * port_width) + 3] : UINT16_MAX, + (l->listener->rc_raw.channel_count > (i * port_width) + 4) ? l->listener->rc_raw.values[(i * port_width) + 4] : UINT16_MAX, + (l->listener->rc_raw.channel_count > (i * port_width) + 5) ? l->listener->rc_raw.values[(i * port_width) + 5] : UINT16_MAX, + (l->listener->rc_raw.channel_count > (i * port_width) + 6) ? l->listener->rc_raw.values[(i * port_width) + 6] : UINT16_MAX, + (l->listener->rc_raw.channel_count > (i * port_width) + 7) ? l->listener->rc_raw.values[(i * port_width) + 7] : UINT16_MAX, + l->listener->rc_raw.rssi); } } } @@ -328,48 +335,48 @@ void MavlinkOrbListener::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->get_mavlink_chan(), - global_pos.timestamp / 1000, - global_pos.lat * 1e7, - global_pos.lon * 1e7, - global_pos.alt * 1000.0f, - (global_pos.alt - home.alt) * 1000.0f, - global_pos.vel_n * 100.0f, - global_pos.vel_e * 100.0f, - global_pos.vel_d * 100.0f, - _wrap_2pi(global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f); + orb_copy(ORB_ID(vehicle_global_position), _mavlink->get_subs()->global_pos_sub, l->listener->global_pos); + + mavlink_msg_global_position_int_send(l->mavlink->get_chan(), + l->listener->global_pos.timestamp / 1000, + l->listener->global_pos.lat * 1e7, + l->listener->global_pos.lon * 1e7, + l->listener->global_pos.alt * 1000.0f, + (l->listener->global_pos.alt - l->listener->home.alt) * 1000.0f, + l->listener->global_pos.vel_n * 100.0f, + l->listener->global_pos.vel_e * 100.0f, + l->listener->global_pos.vel_d * 100.0f, + _wrap_2pi(l->listener->global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f); } void MavlinkOrbListener::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->get_mavlink_chan(), - local_pos.timestamp / 1000, - local_pos.x, - local_pos.y, - local_pos.z, - local_pos.vx, - local_pos.vy, - local_pos.vz); + orb_copy(ORB_ID(vehicle_local_position), _mavlink->get_subs()->local_pos_sub, l->listener->local_pos); + + if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) + mavlink_msg_local_position_ned_send(l->mavlink->get_chan(), + l->listener->local_pos.timestamp / 1000, + l->listener->local_pos.x, + l->listener->local_pos.y, + l->listener->local_pos.z, + l->listener->local_pos.vx, + l->listener->local_pos.vy, + l->listener->local_pos.vz); } void MavlinkOrbListener::l_global_position_setpoint(const struct listener *l) { struct position_setpoint_triplet_s triplet; - orb_copy(ORB_ID(position_setpoint_triplet), mavlink_subs.triplet_sub, &triplet); + orb_copy(ORB_ID(position_setpoint_triplet), l->mavlink->get_subs()->triplet_sub, &triplet); if (!triplet.current.valid) return; - if (gcs_link) - mavlink_msg_global_position_setpoint_int_send(_mavlink->get_mavlink_chan(), + if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) + mavlink_msg_global_position_setpoint_int_send(l->mavlink->get_chan(), MAV_FRAME_GLOBAL, (int32_t)(triplet.current.lat * 1e7d), (int32_t)(triplet.current.lon * 1e7d), @@ -383,10 +390,10 @@ MavlinkOrbListener::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); + orb_copy(ORB_ID(vehicle_local_position_setpoint), l->mavlink->get_subs()->spl_sub, &local_sp); - if (gcs_link) - mavlink_msg_local_position_setpoint_send(_mavlink->get_mavlink_chan(), + if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) + mavlink_msg_local_position_setpoint_send(l->mavlink->get_chan(), MAV_FRAME_LOCAL_NED, local_sp.x, local_sp.y, @@ -400,10 +407,10 @@ MavlinkOrbListener::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); + orb_copy(ORB_ID(vehicle_attitude_setpoint), _mavlink->get_subs()->spa_sub, &att_sp); - if (gcs_link) - mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_mavlink->get_mavlink_chan(), + if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) + mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(l->mavlink->get_chan(), att_sp.timestamp / 1000, att_sp.roll_body, att_sp.pitch_body, @@ -417,10 +424,10 @@ MavlinkOrbListener::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); + orb_copy(ORB_ID(vehicle_rates_setpoint), l->mavlink->get_subs()->rates_setpoint_sub, &rates_sp); - if (gcs_link) - mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_mavlink->get_mavlink_chan(), + if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) + mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(l->mavlink->get_chan(), rates_sp.timestamp / 1000, rates_sp.roll, rates_sp.pitch, @@ -443,8 +450,8 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l) /* 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->get_mavlink_chan(), last_sensor_timestamp / 1000, + if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { + mavlink_msg_servo_output_raw_send(l->mavlink->get_chan(), l->listener->last_sensor_timestamp / 1000, l->arg /* port number - needs GCS support */, /* QGC has port number support already */ act_outputs.output[0], @@ -457,7 +464,7 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l) 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)) { + if (l->listener->mavlink_hil_enabled && l->listener->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; @@ -470,7 +477,7 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l) /* scale / assign outputs depending on system type */ if (mavlink_system.type == MAV_TYPE_QUADROTOR) { - mavlink_msg_hil_controls_send(chan, + mavlink_msg_hil_controls_send(l->mavlink->get_chan(), hrt_absolute_time(), ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, @@ -484,7 +491,7 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l) 0); } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) { - mavlink_msg_hil_controls_send(chan, + mavlink_msg_hil_controls_send(l->mavlink->get_chan(), hrt_absolute_time(), ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, @@ -498,7 +505,7 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l) 0); } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) { - mavlink_msg_hil_controls_send(chan, + mavlink_msg_hil_controls_send(l->mavlink->get_chan(), hrt_absolute_time(), ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, @@ -512,7 +519,7 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l) 0); } else { - mavlink_msg_hil_controls_send(chan, + mavlink_msg_hil_controls_send(l->mavlink->get_chan(), hrt_absolute_time(), (act_outputs.output[0] - 1500.0f) / 500.0f, (act_outputs.output[1] - 1500.0f) / 500.0f, @@ -532,7 +539,7 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l) void MavlinkOrbListener::l_actuator_armed(const struct listener *l) { - orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); + orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed); } void @@ -541,10 +548,10 @@ MavlinkOrbListener::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); + orb_copy(ORB_ID(manual_control_setpoint), l->mavlink->get_subs()->man_control_sp_sub, &man_control); - if (gcs_link) - mavlink_msg_manual_control_send(_mavlink->get_mavlink_chan(), + if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) + mavlink_msg_manual_control_send(l->mavlink->get_chan(), mavlink_system.sysid, man_control.roll * 1000, man_control.pitch * 1000, @@ -556,26 +563,26 @@ MavlinkOrbListener::l_manual_control_setpoint(const struct listener *l) void MavlinkOrbListener::l_vehicle_attitude_controls(const struct listener *l) { - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators_0); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, l->mavlink->get_subs()->actuators_sub, &l->listener->actuators_0); - if (gcs_link) { + if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { /* send, add spaces so that string buffer is at least 10 chars long */ - mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(), - last_sensor_timestamp / 1000, + mavlink_msg_named_value_float_send(l->mavlink->get_chan(), + l->listener->last_sensor_timestamp / 1000, "ctrl0 ", - actuators_0.control[0]); - mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(), - last_sensor_timestamp / 1000, + l->listener->actuators_0.control[0]); + mavlink_msg_named_value_float_send(l->mavlink->get_chan(), + l->listener->last_sensor_timestamp / 1000, "ctrl1 ", - actuators_0.control[1]); - mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(), - last_sensor_timestamp / 1000, + l->listener->actuators_0.control[1]); + mavlink_msg_named_value_float_send(l->mavlink->get_chan(), + l->listener->last_sensor_timestamp / 1000, "ctrl2 ", - actuators_0.control[2]); - mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(), - last_sensor_timestamp / 1000, + l->listener->actuators_0.control[2]); + mavlink_msg_named_value_float_send(l->mavlink->get_chan(), + l->listener->last_sensor_timestamp / 1000, "ctrl3 ", - actuators_0.control[3]); + l->listener->actuators_0.control[3]); } } @@ -584,13 +591,13 @@ MavlinkOrbListener::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); + orb_copy(ORB_ID(debug_key_value), l->mavlink->get_subs()->debug_key_value, &debug); /* Enforce null termination */ debug.key[sizeof(debug.key) - 1] = '\0'; - mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(), - last_sensor_timestamp / 1000, + mavlink_msg_named_value_float_send(l->mavlink->get_chan(), + l->listener->last_sensor_timestamp / 1000, debug.key, debug.value); } @@ -600,52 +607,52 @@ MavlinkOrbListener::l_optical_flow(const struct listener *l) { struct optical_flow_s flow; - orb_copy(ORB_ID(optical_flow), mavlink_subs.optical_flow, &flow); + orb_copy(ORB_ID(optical_flow), l->mavlink->get_subs()->optical_flow, &flow); - mavlink_msg_optical_flow_send(_mavlink->get_mavlink_chan(), flow.timestamp, flow.sensor_id, flow.flow_raw_x, flow.flow_raw_y, + mavlink_msg_optical_flow_send(l->mavlink->get_chan(), 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 MavlinkOrbListener::l_home(const struct listener *l) { - orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home); + orb_copy(ORB_ID(home_position), l->mavlink->get_subs()->home_sub, &home); - mavlink_msg_gps_global_origin_send(_mavlink->get_mavlink_chan(), (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.alt)*1e3f); + mavlink_msg_gps_global_origin_send(l->mavlink->get_chan(), (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.alt)*1e3f); } void MavlinkOrbListener::l_airspeed(const struct listener *l) { - orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed); + orb_copy(ORB_ID(airspeed), l->mavlink->get_subs()->airspeed_sub, &l->listener->airspeed); } void MavlinkOrbListener::l_nav_cap(const struct listener *l) { - orb_copy(ORB_ID(navigation_capabilities), mavlink_subs.navigation_capabilities_sub, &nav_cap); + orb_copy(ORB_ID(navigation_capabilities), l->mavlink->get_subs()->navigation_capabilities_sub, &l->listener->nav_cap); - mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(), + mavlink_msg_named_value_float_send(l->mavlink->get_chan(), hrt_absolute_time() / 1000, "turn dist", - nav_cap.turn_distance); + l->listener->nav_cap.turn_distance); } void MavlinkOrbListener::l_control_mode(const struct listener *l) { - orb_copy(ORB_ID(vehicle_control_mode), mavlink_subs.control_mode_sub, &control_mode); + orb_copy(ORB_ID(vehicle_control_mode), l->mavlink->get_subs()->control_mode_sub, &l->listener->control_mode); /* 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); + l->mavlink->get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, + mavlink_msg_heartbeat_send(l->mavlink->get_chan(), mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, @@ -657,7 +664,9 @@ void * MavlinkOrbListener::uorb_receive_thread(void *arg) { /* Set thread name */ - prctl(PR_SET_NAME, "mavlink_orb_rcv", getpid()); + char buf[32]; + sprintf(buf, "mavlink rcv%d", Mavlink::instance_count()); + prctl(PR_SET_NAME, buf, getpid()); /* * set up poll to block for new data, @@ -670,15 +679,21 @@ MavlinkOrbListener::uorb_receive_thread(void *arg) * * Might want to invoke each listener once to set initial state. */ - struct pollfd fds[_n_listeners]; + struct pollfd fds[_max_listeners]; + + struct listener* next = _listeners; + unsigned i = 0; - for (unsigned i = 0; i < _n_listeners; i++) { - fds[i].fd = *listeners[i].subp; + while (next != nullptr) { + + fds[i].fd = *next->subp; fds[i].events = POLLIN; + next = next->next; + i++; + } /* Invoke callback to set initial state */ //listeners[i].callback(&listener[i]); - } while (!thread_should_exit) { @@ -689,13 +704,19 @@ MavlinkOrbListener::uorb_receive_thread(void *arg) /* silent */ } else if (poll_ret < 0) { - mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data"); + //mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data"); } else { - for (unsigned i = 0; i < _n_listeners; i++) { + unsigned i = 0; + struct listener* cb = _listeners; + while (cb != nullptr) { + if (fds[i].revents & POLLIN) - listeners[i].callback(&listeners[i]); + cb->callback(cb); + + cb = cb->next; + i++; } } } @@ -703,107 +724,113 @@ MavlinkOrbListener::uorb_receive_thread(void *arg) return NULL; } +void * MavlinkOrbListener::uorb_start_helper(void *context) +{ + return ((MavlinkOrbListener *)context)->uorb_receive_thread(NULL); +} + pthread_t -MavlinkOrbListener::uorb_receive_start(void) +MavlinkOrbListener::uorb_receive_start(Mavlink* mavlink) { + MavlinkOrbListener* urcv = new MavlinkOrbListener(mavlink); + /* --- SENSORS RAW VALUE --- */ - mavlink_subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + mavlink->get_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 */ + orb_set_interval(mavlink->get_subs()->sensor_sub, 200); /* 5Hz updates */ /* --- ATTITUDE VALUE --- */ - mavlink_subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + mavlink->get_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 */ + orb_set_interval(mavlink->get_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 */ + mavlink->get_subs()->gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + orb_set_interval(mavlink->get_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 */ + mavlink->get_subs()->home_sub = orb_subscribe(ORB_ID(home_position)); + orb_set_interval(mavlink->get_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 */ + mavlink->get_subs()->status_sub = orb_subscribe(ORB_ID(vehicle_status)); + orb_set_interval(mavlink->get_subs()->status_sub, 300); /* max 3.33 Hz updates */ /* --- CONTROL MODE --- */ - mavlink_subs.control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - orb_set_interval(mavlink_subs.control_mode_sub, 300); /* max 3.33 Hz updates */ + mavlink->get_subs()->control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + orb_set_interval(mavlink->get_subs()->control_mode_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 */ + mavlink->get_subs()->rc_sub = orb_subscribe(ORB_ID(rc_channels)); + orb_set_interval(mavlink->get_subs()->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); + mavlink->get_subs()->input_rc_sub = orb_subscribe(ORB_ID(input_rc)); + orb_set_interval(mavlink->get_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 */ + mavlink->get_subs()->global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + orb_set_interval(mavlink->get_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 */ + mavlink->get_subs()->local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + orb_set_interval(mavlink->get_subs()->local_pos_sub, 1000); /* 1Hz active updates */ /* --- GLOBAL SETPOINT VALUE --- */ - mavlink_subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); - orb_set_interval(mavlink_subs.triplet_sub, 2000); /* 0.5 Hz updates */ + mavlink->get_subs()->triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); + orb_set_interval(mavlink->get_subs()->triplet_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 */ + mavlink->get_subs()->spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); + orb_set_interval(mavlink->get_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 */ + mavlink->get_subs()->spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + orb_set_interval(mavlink->get_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 */ + mavlink->get_subs()->rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + orb_set_interval(mavlink->get_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)); + mavlink->get_subs()->act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0)); + mavlink->get_subs()->act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1)); + mavlink->get_subs()->act_2_sub = orb_subscribe(ORB_ID(actuator_outputs_2)); + mavlink->get_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 */ + orb_set_interval(mavlink->get_subs()->act_0_sub, 100); /* 10Hz updates */ + orb_set_interval(mavlink->get_subs()->act_1_sub, 100); /* 10Hz updates */ + orb_set_interval(mavlink->get_subs()->act_2_sub, 100); /* 10Hz updates */ + orb_set_interval(mavlink->get_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 */ + mavlink->get_subs()->armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(mavlink->get_subs()->armed_sub, 100); /* 10Hz updates */ /* --- MAPPED MANUAL CONTROL INPUTS --- */ - mavlink_subs.man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + mavlink->get_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 */ + orb_set_interval(mavlink->get_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 */ + mavlink->get_subs()->actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + orb_set_interval(mavlink->get_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 */ + mavlink->get_subs()->debug_key_value = orb_subscribe(ORB_ID(debug_key_value)); + orb_set_interval(mavlink->get_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 */ + mavlink->get_subs()->optical_flow = orb_subscribe(ORB_ID(optical_flow)); + orb_set_interval(mavlink->get_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 */ + mavlink->get_subs()->airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + orb_set_interval(mavlink->get_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; + mavlink->get_subs()->navigation_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); + orb_set_interval(mavlink->get_subs()->navigation_capabilities_sub, 500); /* 2Hz updates */ /* start the listener loop */ pthread_attr_t uorb_attr; @@ -813,7 +840,7 @@ MavlinkOrbListener::uorb_receive_start(void) pthread_attr_setstacksize(&uorb_attr, 2048); pthread_t thread; - pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL); + pthread_create(&thread, &uorb_attr, MavlinkOrbListener::uorb_start_helper, urcv); pthread_attr_destroy(&uorb_attr); return thread; diff --git a/src/modules/mavlink/mavlink_orb_listener.h b/src/modules/mavlink/mavlink_orb_listener.h index 29e081b36..3988103bc 100644 --- a/src/modules/mavlink/mavlink_orb_listener.h +++ b/src/modules/mavlink/mavlink_orb_listener.h @@ -38,8 +38,40 @@ * @author Lorenz Meier */ +#include + #pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + class Mavlink; class MavlinkOrbListener @@ -67,18 +99,32 @@ public: */ void status(); - pthread_t uorb_receive_start(void); + static pthread_t uorb_receive_start(Mavlink *mavlink); void * uorb_receive_thread(void *arg); + struct listener { + void (*callback)(const struct listener *l); + int *subp; + uintptr_t arg; + struct listener* next; + Mavlink *mavlink; + MavlinkOrbListener* listener; + }; + + void add_listener(void (*callback)(const struct listener *l), int *subp, uintptr_t arg); + static void * uorb_start_helper(void *context); + private: - bool _task_should_exit; /**< if true, sensor task should exit */ + bool thread_should_exit; /**< if true, sensor task should exit */ perf_counter_t _loop_perf; /**< loop performance counter */ Mavlink* _mavlink; + struct listener *_listeners; unsigned _n_listeners; + static const unsigned _max_listeners = 32; /** * Shim for calling task_main from task_create. @@ -90,28 +136,28 @@ private: */ void task_main() __attribute__((noreturn)); - void l_sensor_combined(const struct listener *l); - void l_vehicle_attitude(const struct listener *l); - void l_vehicle_gps_position(const struct listener *l); - void l_vehicle_status(const struct listener *l); - void l_rc_channels(const struct listener *l); - void l_input_rc(const struct listener *l); - void l_global_position(const struct listener *l); - void l_local_position(const struct listener *l); - void l_global_position_setpoint(const struct listener *l); - void l_local_position_setpoint(const struct listener *l); - void l_attitude_setpoint(const struct listener *l); - void l_actuator_outputs(const struct listener *l); - void l_actuator_armed(const struct listener *l); - void l_manual_control_setpoint(const struct listener *l); - void l_vehicle_attitude_controls(const struct listener *l); - void l_debug_key_value(const struct listener *l); - void l_optical_flow(const struct listener *l); - void l_vehicle_rates_setpoint(const struct listener *l); - void l_home(const struct listener *l); - void l_airspeed(const struct listener *l); - void l_nav_cap(const struct listener *l); - void l_control_mode(const struct listener *l); + 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 void l_control_mode(const struct listener *l); struct vehicle_global_position_s global_pos; struct vehicle_local_position_s local_pos; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 4f763c3c6..982d6c1d8 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -83,7 +83,8 @@ __BEGIN_DECLS __END_DECLS -void MavlinkReceiver::MavlinkReceiver() : +MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : + _mavlink(parent), pub_hil_global_pos(-1), pub_hil_local_pos(-1), pub_hil_attitude(-1), @@ -790,7 +791,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) void * MavlinkReceiver::receive_thread(void *arg) { - int uart_fd = *((int *)arg); + int uart_fd = _mavlink->get_uart_fd(); const int timeout = 1000; uint8_t buf[32]; @@ -822,10 +823,10 @@ MavlinkReceiver::receive_thread(void *arg) handle_message(&msg); /* handle packet with waypoint component */ - mavlink_wpm_message_handler(&msg); + _mavlink->mavlink_wpm_message_handler(&msg); /* handle packet with parameter component */ - mavlink_pm_message_handler(MAVLINK_COMM_0, &msg); + _mavlink->mavlink_pm_message_handler(MAVLINK_COMM_0, &msg); } } } @@ -839,15 +840,22 @@ void MavlinkReceiver::print_status() } +void * MavlinkReceiver::start_helper(void *context) +{ + return ((MavlinkReceiver *)context)->receive_thread(NULL); +} + pthread_t -MavlinkReceiver::receive_start(int uart) +MavlinkReceiver::receive_start(Mavlink *mavlink) { + MavlinkReceiver *rcv = new MavlinkReceiver(mavlink); + pthread_attr_t receiveloop_attr; pthread_attr_init(&receiveloop_attr); // set to non-blocking read - int flags = fcntl(uart, F_GETFL, 0); - fcntl(uart, F_SETFL, flags | O_NONBLOCK); + int flags = fcntl(mavlink->get_uart_fd(), F_GETFL, 0); + fcntl(mavlink->get_uart_fd(), F_SETFL, flags | O_NONBLOCK); struct sched_param param; (void)pthread_attr_getschedparam(&receiveloop_attr, ¶m); @@ -857,7 +865,7 @@ MavlinkReceiver::receive_start(int uart) pthread_attr_setstacksize(&receiveloop_attr, 3000); pthread_t thread; - pthread_create(&thread, &receiveloop_attr, receive_thread, &uart); + pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, rcv); pthread_attr_destroy(&receiveloop_attr); return thread; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index ea57714d2..483d91e72 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -96,7 +96,9 @@ public: */ void print_status(); - pthread_t receive_start(int uart); + static pthread_t receive_start(Mavlink* mavlink); + + static void * start_helper(void *context); private: -- cgit v1.2.3