From 34da0d85d03c0690cc13bd21511bca3bb2f6b8a6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Jul 2017 18:28:02 +0200 Subject: [PATCH] MAVLink: Improve message handling / tracking The message handling was not obeying action focused messages and high-rate messages properly before. With this change update rates track the desired rates closely. Critical high-rate messages such as ADS-B are queued additionally to guarantee that all received packets are being correctly forwarded. --- msg/transponder_report.msg | 2 +- src/modules/mavlink/mavlink_main.cpp | 147 ++++++----- src/modules/mavlink/mavlink_main.h | 25 +- src/modules/mavlink/mavlink_messages.cpp | 236 ++++++++++++++---- .../mavlink/mavlink_orb_subscription.h | 2 + src/modules/mavlink/mavlink_receiver.cpp | 2 +- src/modules/mavlink/mavlink_stream.cpp | 22 +- src/modules/mavlink/mavlink_stream.h | 2 +- 8 files changed, 310 insertions(+), 128 deletions(-) diff --git a/msg/transponder_report.msg b/msg/transponder_report.msg index 3d13a42d4a8d..fb249c58a534 100644 --- a/msg/transponder_report.msg +++ b/msg/transponder_report.msg @@ -12,4 +12,4 @@ uint8 tslc # Time since last communication in seconds uint16 flags # Flags to indicate various statuses including valid data fields uint16 squawk # Squawk code -uint32 ORB_QUEUE_LENGTH = 3 +uint32 ORB_QUEUE_LENGTH = 10 diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 36a8bf5d684d..fd974ca8e82c 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1348,23 +1348,31 @@ MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, int return sub_new; } -unsigned int +int Mavlink::interval_from_rate(float rate) { - return (rate > 0.0f) ? (1000000.0f / rate) : 0; + if (rate > 0.000001f) { + return (1000000.0f / rate); + + } else if (rate < 0.0f) { + return -1; + + } else { + return 0; + } } int Mavlink::configure_stream(const char *stream_name, const float rate) { - /* calculate interval in us, 0 means disabled stream */ - unsigned int interval = interval_from_rate(rate); + /* calculate interval in us, -1 means unlimited stream, 0 means disabled */ + int interval = interval_from_rate(rate); /* search if stream exists */ MavlinkStream *stream; LL_FOREACH(_streams, stream) { if (strcmp(stream_name, stream->get_name()) == 0) { - if (interval > 0) { + if (interval != 0) { /* set new interval */ stream->set_interval(interval); @@ -1378,7 +1386,7 @@ Mavlink::configure_stream(const char *stream_name, const float rate) } } - if (interval <= 0) { + if (interval == 0) { /* stream was not active and is requested to be disabled, do nothing */ return OK; } @@ -1983,7 +1991,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("SERVO_OUTPUT_RAW_0", 1.0f); configure_stream("ALTITUDE", 1.0f); configure_stream("GPS_RAW_INT", 1.0f); - configure_stream("ADSB_VEHICLE", 2.0f); + configure_stream("ADSB_VEHICLE"); configure_stream("COLLISION", 2.0f); configure_stream("DISTANCE_SENSOR", 0.5f); configure_stream("OPTICAL_FLOW_RAD", 1.0f); @@ -1999,19 +2007,20 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("NAMED_VALUE_FLOAT", 1.0f); configure_stream("VFR_HUD", 4.0f); configure_stream("WIND_COV", 1.0f); + configure_stream("CAMERA_IMAGE_CAPTURED"); break; case MAVLINK_MODE_ONBOARD: configure_stream("SYS_STATUS", 5.0f); configure_stream("EXTENDED_SYS_STATE", 5.0f); configure_stream("HIGHRES_IMU", 50.0f); - configure_stream("ATTITUDE", 250.0f); + configure_stream("ATTITUDE", 100.0f); configure_stream("ATTITUDE_QUATERNION", 50.0f); configure_stream("RC_CHANNELS", 20.0f); configure_stream("SERVO_OUTPUT_RAW_0", 10.0f); configure_stream("ALTITUDE", 10.0f); configure_stream("GPS_RAW_INT", 5.0f); - configure_stream("ADSB_VEHICLE", 10.0f); + configure_stream("ADSB_VEHICLE"); configure_stream("COLLISION", 10.0f); configure_stream("DISTANCE_SENSOR", 10.0f); configure_stream("OPTICAL_FLOW_RAD", 10.0f); @@ -2030,9 +2039,8 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("SYSTEM_TIME", 1.0f); configure_stream("TIMESYNC", 10.0f); configure_stream("CAMERA_CAPTURE", 2.0f); - //camera trigger is rate limited at the source, do not limit here - configure_stream("CAMERA_TRIGGER", 500.0f); - configure_stream("CAMERA_IMAGE_CAPTURED", 5.0f); + configure_stream("CAMERA_TRIGGER"); + configure_stream("CAMERA_IMAGE_CAPTURED"); configure_stream("ACTUATOR_CONTROL_TARGET0", 10.0f); break; @@ -2069,7 +2077,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("SERVO_OUTPUT_RAW_1", 20.0f); configure_stream("ALTITUDE", 10.0f); configure_stream("GPS_RAW_INT", 10.0f); - configure_stream("ADSB_VEHICLE", 20.0f); + configure_stream("ADSB_VEHICLE"); configure_stream("COLLISION", 20.0f); configure_stream("DISTANCE_SENSOR", 10.0f); configure_stream("OPTICAL_FLOW_RAD", 10.0f); @@ -2084,8 +2092,8 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("NAMED_VALUE_FLOAT", 50.0f); configure_stream("VFR_HUD", 20.0f); configure_stream("WIND_COV", 10.0f); - configure_stream("CAMERA_TRIGGER", 500.0f); - configure_stream("CAMERA_IMAGE_CAPTURED", 5.0f); + configure_stream("CAMERA_TRIGGER"); + configure_stream("CAMERA_IMAGE_CAPTURED"); configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f); configure_stream("MANUAL_CONTROL", 5.0f); break; @@ -2101,14 +2109,14 @@ Mavlink::task_main(int argc, char *argv[]) /* set main loop delay depending on data rate to minimize CPU overhead */ _main_loop_delay = (MAIN_LOOP_DELAY * 1000) / _datarate; - /* hard limit to 500 Hz at max */ - if (_main_loop_delay < 2000) { - _main_loop_delay = 2000; + /* hard limit to 1000 Hz at max */ + if (_main_loop_delay < MAVLINK_MIN_INTERVAL) { + _main_loop_delay = MAVLINK_MIN_INTERVAL; } /* hard limit to 100 Hz at least */ - if (_main_loop_delay > 10000) { - _main_loop_delay = 10000; + if (_main_loop_delay > MAVLINK_MAX_INTERVAL) { + _main_loop_delay = MAVLINK_MAX_INTERVAL; } /* now the instance is fully initialized and we can bump the instance count */ @@ -2139,53 +2147,7 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_update_system(); } - /* radio config check */ - if (_uart_fd >= 0 && _radio_id != 0 && _rstatus.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) { - /* request to configure radio and radio is present */ - FILE *fs = fdopen(_uart_fd, "w"); - - if (fs) { - /* switch to AT command mode */ - usleep(1200000); - fprintf(fs, "+++\n"); - usleep(1200000); - - if (_radio_id > 0) { - /* set channel */ - fprintf(fs, "ATS3=%u\n", _radio_id); - usleep(200000); - - } else { - /* reset to factory defaults */ - fprintf(fs, "AT&F\n"); - usleep(200000); - } - - /* write config */ - fprintf(fs, "AT&W"); - usleep(200000); - - /* reboot */ - fprintf(fs, "ATZ"); - usleep(200000); - - // XXX NuttX suffers from a bug where - // fclose() also closes the fd, not just - // the file stream. Since this is a one-time - // config thing, we leave the file struct - // allocated. -#ifndef __PX4_NUTTX - fclose(fs); -#endif - - } else { - PX4_WARN("open fd %d failed", _uart_fd); - } - - /* reset param and save */ - _radio_id = 0; - param_set(_param_radio_id, &_radio_id); - } + check_radio_config(); if (status_sub->update(&status_time, &status)) { /* switch HIL mode if required */ @@ -2414,6 +2376,57 @@ Mavlink::task_main(int argc, char *argv[]) return OK; } +void Mavlink::check_radio_config() +{ + /* radio config check */ + if (_uart_fd >= 0 && _radio_id != 0 && _rstatus.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) { + /* request to configure radio and radio is present */ + FILE *fs = fdopen(_uart_fd, "w"); + + if (fs) { + /* switch to AT command mode */ + usleep(1200000); + fprintf(fs, "+++\n"); + usleep(1200000); + + if (_radio_id > 0) { + /* set channel */ + fprintf(fs, "ATS3=%u\n", _radio_id); + usleep(200000); + + } else { + /* reset to factory defaults */ + fprintf(fs, "AT&F\n"); + usleep(200000); + } + + /* write config */ + fprintf(fs, "AT&W"); + usleep(200000); + + /* reboot */ + fprintf(fs, "ATZ"); + usleep(200000); + + // XXX NuttX suffers from a bug where + // fclose() also closes the fd, not just + // the file stream. Since this is a one-time + // config thing, we leave the file struct + // allocated. +#ifndef __PX4_NUTTX + fclose(fs); +#endif + + } else { + PX4_WARN("open fd %d failed", _uart_fd); + } + + /* reset param and save */ + _radio_id = 0; + param_set(_param_radio_id, &_radio_id); + } +} + int Mavlink::start_helper(int argc, char *argv[]) { /* create the instance in task context */ diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 62c05f83ee07..4e158ef34555 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -41,6 +41,8 @@ #pragma once +#include + #include #ifdef __PX4_NUTTX #include @@ -309,7 +311,7 @@ class Mavlink mavlink_channel_t get_channel(); - void configure_stream_threadsafe(const char *stream_name, float rate); + void configure_stream_threadsafe(const char *stream_name, float rate = -1.0f); bool _task_should_exit; /**< if true, mavlink task should exit */ @@ -431,7 +433,7 @@ class Mavlink bool verbose() { return _verbose; } - int get_data_rate() { return _datarate; } + int get_data_rate() { return _datarate; } void set_data_rate(int rate) { if (rate > 0) { _datarate = rate; } } uint64_t get_main_loop_delay() { return _main_loop_delay; } @@ -455,6 +457,9 @@ class Mavlink if (_mavlink_ulog) { _mavlink_ulog_stop_requested = true; } } + + void set_uorb_main_fd(int fd, unsigned int interval); + protected: Mavlink *next; @@ -464,7 +469,9 @@ class Mavlink orb_advert_t _mavlink_log_pub; bool _task_running; static bool _boot_complete; - static const unsigned MAVLINK_MAX_INSTANCES = 4; + static constexpr unsigned MAVLINK_MAX_INSTANCES = 4; + static constexpr unsigned MAVLINK_MIN_INTERVAL = 1500; + static constexpr unsigned MAVLINK_MAX_INTERVAL = 10000; mavlink_message_t _mavlink_buffer; mavlink_status_t _mavlink_status; @@ -590,13 +597,13 @@ class Mavlink int mavlink_open_uart(int baudrate, const char *uart_name); #endif - static unsigned int interval_from_rate(float rate); + static int interval_from_rate(float rate); static constexpr unsigned RADIO_BUFFER_CRITICAL_LOW_PERCENTAGE = 25; static constexpr unsigned RADIO_BUFFER_LOW_PERCENTAGE = 35; static constexpr unsigned RADIO_BUFFER_HALF_PERCENTAGE = 50; - int configure_stream(const char *stream_name, const float rate); + int configure_stream(const char *stream_name, const float rate = -1.0f); /** * Adjust the stream rates based on the current rate @@ -619,6 +626,14 @@ class Mavlink void pass_message(const mavlink_message_t *msg); + /** + * Check the configuration of a connected radio + * + * This convenience function allows to re-configure a connected + * radio without removing it from the main system harness. + */ + void check_radio_config(); + /** * Update rate mult so total bitrate will be equal to _datarate. */ diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 2db6551b2501..805110114200 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -318,7 +318,7 @@ class MavlinkStreamHeartbeat : public MavlinkStream _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))) {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { struct vehicle_status_s status = {}; @@ -335,6 +335,8 @@ class MavlinkStreamHeartbeat : public MavlinkStream mavlink_msg_heartbeat_send(_mavlink->get_channel(), _mavlink->get_system_type(), MAV_AUTOPILOT_PX4, base_mode, custom_mode, system_status); + + return true; } }; @@ -383,7 +385,7 @@ class MavlinkStreamStatustext : public MavlinkStream ~MavlinkStreamStatustext() {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { if (!_mavlink->get_logbuffer()->empty() && _mavlink->is_connected()) { @@ -397,8 +399,12 @@ class MavlinkStreamStatustext : public MavlinkStream msg.text[sizeof(msg.text) - 1] = '\0'; mavlink_msg_statustext_send_struct(_mavlink->get_channel(), &msg); + + return true; } } + + return false; } }; @@ -449,9 +455,10 @@ class MavlinkStreamCommandLong : public MavlinkStream _cmd_time(0) {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { struct vehicle_command_s cmd; + bool sent = false; if (_cmd_sub->update(&_cmd_time, &cmd)) { @@ -464,6 +471,7 @@ class MavlinkStreamCommandLong : public MavlinkStream } MavlinkCommandSender::instance().handle_vehicle_command(cmd, _mavlink->get_channel()); + sent = true; } else { if (_mavlink->verbose()) { @@ -473,6 +481,8 @@ class MavlinkStreamCommandLong : public MavlinkStream } MavlinkCommandSender::instance().check_timeout(_mavlink->get_channel()); + + return sent; } }; @@ -525,7 +535,7 @@ class MavlinkStreamSysStatus : public MavlinkStream _battery_status_sub(_mavlink->add_orb_subscription(ORB_ID(battery_status))) {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { struct vehicle_status_s status = {}; struct cpuload_s cpuload = {}; @@ -576,7 +586,11 @@ class MavlinkStreamSysStatus : public MavlinkStream } mavlink_msg_battery_status_send_struct(_mavlink->get_channel(), &bat_msg); + + return true; } + + return false; } }; @@ -642,7 +656,7 @@ class MavlinkStreamHighresIMU : public MavlinkStream _baro_timestamp(0) {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { struct sensor_combined_s sensor = {}; struct differential_pressure_s differential_pressure = {}; @@ -695,7 +709,11 @@ class MavlinkStreamHighresIMU : public MavlinkStream msg.fields_updated = fields_updated; mavlink_msg_highres_imu_send_struct(_mavlink->get_channel(), &msg); + + return true; } + + return false; } }; @@ -748,7 +766,7 @@ class MavlinkStreamAttitude : public MavlinkStream _att_time(0) {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { struct vehicle_attitude_s att; @@ -764,7 +782,11 @@ class MavlinkStreamAttitude : public MavlinkStream msg.yawspeed = att.yawspeed; mavlink_msg_attitude_send_struct(_mavlink->get_channel(), &msg); + + return true; } + + return false; } }; @@ -816,7 +838,7 @@ class MavlinkStreamAttitudeQuaternion : public MavlinkStream _att_time(0) {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { struct vehicle_attitude_s att; @@ -833,7 +855,11 @@ class MavlinkStreamAttitudeQuaternion : public MavlinkStream msg.yawspeed = att.yawspeed; mavlink_msg_attitude_quaternion_send_struct(_mavlink->get_channel(), &msg); + + return true; } + + return false; } }; @@ -916,7 +942,7 @@ class MavlinkStreamVFRHUD : public MavlinkStream _sensor_time(0) {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { struct vehicle_attitude_s att = {}; struct vehicle_global_position_s pos = {}; @@ -966,7 +992,11 @@ class MavlinkStreamVFRHUD : public MavlinkStream msg.climb = -pos.vel_d; mavlink_msg_vfr_hud_send_struct(_mavlink->get_channel(), &msg); + + return true; } + + return false; } }; @@ -1018,7 +1048,7 @@ class MavlinkStreamGPSRawInt : public MavlinkStream _gps_time(0) {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { struct vehicle_gps_position_s gps; @@ -1042,7 +1072,11 @@ class MavlinkStreamGPSRawInt : public MavlinkStream msg.satellites_visible = gps.satellites_used; mavlink_msg_gps_raw_int_send_struct(_mavlink->get_channel(), &msg); + + return true; } + + return false; } }; @@ -1088,7 +1122,7 @@ class MavlinkStreamSystemTime : public MavlinkStream explicit MavlinkStreamSystemTime(Mavlink *mavlink) : MavlinkStream(mavlink) {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { mavlink_system_time_t msg = {}; timespec tv; @@ -1099,6 +1133,8 @@ class MavlinkStreamSystemTime : public MavlinkStream msg.time_unix_usec = (uint64_t)tv.tv_sec * 1000000 + tv.tv_nsec / 1000; mavlink_msg_system_time_send_struct(_mavlink->get_channel(), &msg); + + return true; } }; @@ -1144,7 +1180,7 @@ class MavlinkStreamTimesync : public MavlinkStream explicit MavlinkStreamTimesync(Mavlink *mavlink) : MavlinkStream(mavlink) {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { mavlink_timesync_t msg = {}; @@ -1152,6 +1188,8 @@ class MavlinkStreamTimesync : public MavlinkStream msg.ts1 = hrt_absolute_time() * 1000; // boot time in nanoseconds mavlink_msg_timesync_send_struct(_mavlink->get_channel(), &msg); + + return true; } }; @@ -1183,7 +1221,7 @@ class MavlinkStreamADSBVehicle : public MavlinkStream return new MavlinkStreamADSBVehicle(mavlink); } - virtual bool const_rate() + bool const_rate() { return true; } @@ -1207,9 +1245,10 @@ class MavlinkStreamADSBVehicle : public MavlinkStream _pos_time(0) {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { struct transponder_report_s pos; + bool sent = false; while (_pos_sub->update(&_pos_time, &pos)) { mavlink_adsb_vehicle_t msg = {}; @@ -1229,7 +1268,10 @@ class MavlinkStreamADSBVehicle : public MavlinkStream msg.squawk = pos.squawk; mavlink_msg_adsb_vehicle_send_struct(_mavlink->get_channel(), &msg); + sent = true; } + + return sent; } }; @@ -1280,11 +1322,12 @@ class MavlinkStreamCollision : public MavlinkStream _collision_time(0) {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { struct collision_report_s report; + bool sent = false; - if (_collision_sub->update(&_collision_time, &report)) { + while (_collision_sub->update(&_collision_time, &report)) { mavlink_collision_t msg = {}; msg.src = report.src; @@ -1296,7 +1339,10 @@ class MavlinkStreamCollision : public MavlinkStream msg.horizontal_minimum_delta = report.horizontal_minimum_delta; mavlink_msg_collision_send_struct(_mavlink->get_channel(), &msg); + sent = true; } + + return sent; } }; @@ -1328,7 +1374,7 @@ class MavlinkStreamCameraTrigger : public MavlinkStream return new MavlinkStreamCameraTrigger(mavlink); } - virtual bool const_rate() + bool const_rate() { return true; } @@ -1352,7 +1398,7 @@ class MavlinkStreamCameraTrigger : public MavlinkStream _trigger_time(0) {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { struct camera_trigger_s trigger; @@ -1400,8 +1446,12 @@ class MavlinkStreamCameraTrigger : public MavlinkStream digicam_ctrl_cmd.param7 = NAN; mavlink_msg_command_long_send_struct(_mavlink->get_channel(), &digicam_ctrl_cmd); + + return true; } } + + return false; } }; @@ -1457,7 +1507,7 @@ class MavlinkStreamCameraImageCaptured : public MavlinkStream _capture_time(0) {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { struct camera_capture_s capture; @@ -1481,7 +1531,11 @@ class MavlinkStreamCameraImageCaptured : public MavlinkStream msg.file_url[0] = '\0'; mavlink_msg_camera_image_captured_send_struct(_mavlink->get_channel(), &msg); + + return true; } + + return false; } }; @@ -1537,7 +1591,7 @@ class MavlinkStreamGlobalPositionInt : public MavlinkStream _home_time(0) {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { struct vehicle_global_position_s pos = {}; struct home_position_s home = {}; @@ -1560,6 +1614,8 @@ class MavlinkStreamGlobalPositionInt : public MavlinkStream mavlink_msg_global_position_int_send_struct(_mavlink->get_channel(), &msg); } + + return updated; } }; @@ -1615,7 +1671,7 @@ class MavlinkStreamVisionPositionEstimate : public MavlinkStream _att_time(0) {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { struct vehicle_local_position_s vpos = {}; struct vehicle_attitude_s vatt = {}; @@ -1637,6 +1693,8 @@ class MavlinkStreamVisionPositionEstimate : public MavlinkStream mavlink_msg_vision_position_estimate_send_struct(_mavlink->get_channel(), &vmsg); } + + return (pos_updated || att_updated); } }; @@ -1687,7 +1745,7 @@ class MavlinkStreamLocalPositionNED : public MavlinkStream _pos_time(0) {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { struct vehicle_local_position_s pos; @@ -1703,7 +1761,11 @@ class MavlinkStreamLocalPositionNED : public MavlinkStream msg.vz = pos.vz; mavlink_msg_local_position_ned_send_struct(_mavlink->get_channel(), &msg); + + return true; } + + return false; } }; @@ -1755,7 +1817,7 @@ class MavlinkStreamLocalPositionNEDCOV : public MavlinkStream _est_time(0) {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { struct estimator_status_s est = {}; @@ -1782,7 +1844,11 @@ class MavlinkStreamLocalPositionNEDCOV : public MavlinkStream msg.covariance[11] = est.timeout_flags; mavlink_msg_local_position_ned_cov_send_struct(_mavlink->get_channel(), &msg); + + return true; } + + return false; } }; @@ -1833,7 +1899,7 @@ class MavlinkStreamEstimatorStatus : public MavlinkStream _est_time(0) {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { struct estimator_status_s est; @@ -1863,7 +1929,11 @@ class MavlinkStreamEstimatorStatus : public MavlinkStream msg.vibration_z = est.vibe[2]; mavlink_msg_vibration_send_struct(_mavlink->get_channel(), &msg); + + return true; } + + return false; } }; @@ -1914,7 +1984,7 @@ class MavlinkStreamAttPosMocap : public MavlinkStream _mocap_time(0) {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { struct att_pos_mocap_s mocap; @@ -1931,7 +2001,11 @@ class MavlinkStreamAttPosMocap : public MavlinkStream msg.z = mocap.z; mavlink_msg_att_pos_mocap_send_struct(_mavlink->get_channel(), &msg); + + return true; } + + return false; } }; @@ -1981,7 +2055,7 @@ class MavlinkStreamHomePosition : public MavlinkStream _home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))) {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { /* we're sending the GPS home periodically to ensure the * the GCS does pick it up at one point */ @@ -2012,8 +2086,12 @@ class MavlinkStreamHomePosition : public MavlinkStream msg.approach_z = 0.0f; mavlink_msg_home_position_send_struct(_mavlink->get_channel(), &msg); + + return true; } } + + return false; } }; @@ -2080,7 +2158,7 @@ class MavlinkStreamServoOutputRaw : public MavlinkStream _act_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_outputs), N); } - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { struct actuator_outputs_s act; @@ -2099,7 +2177,11 @@ class MavlinkStreamServoOutputRaw : public MavlinkStream msg.servo8_raw = act.output[7]; mavlink_msg_servo_output_raw_send_struct(_mavlink->get_channel(), &msg); + + return true; } + + return false; } }; @@ -2182,7 +2264,7 @@ class MavlinkStreamActuatorControlTarget : public MavlinkStream } } - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { struct actuator_controls_s att_ctrl; @@ -2197,7 +2279,11 @@ class MavlinkStreamActuatorControlTarget : public MavlinkStream } mavlink_msg_actuator_control_target_send_struct(_mavlink->get_channel(), &msg); + + return true; } + + return false; } }; @@ -2255,7 +2341,7 @@ class MavlinkStreamHILControls : public MavlinkStream _act_time(0) {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { struct vehicle_status_s status; struct actuator_outputs_s act; @@ -2363,6 +2449,8 @@ class MavlinkStreamHILControls : public MavlinkStream mavlink_msg_hil_controls_send_struct(_mavlink->get_channel(), &msg); } + + return updated; } }; @@ -2418,7 +2506,7 @@ class MavlinkStreamHILActuatorControls : public MavlinkStream _act_time(0) {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { struct vehicle_status_s status; struct actuator_outputs_s act; @@ -2521,6 +2609,8 @@ class MavlinkStreamHILActuatorControls : public MavlinkStream mavlink_msg_hil_actuator_controls_send_struct(_mavlink->get_channel(), &msg); } + + return (updated && (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)); } }; @@ -2569,7 +2659,7 @@ class MavlinkStreamPositionTargetGlobalInt : public MavlinkStream _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))) {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { struct position_setpoint_triplet_s pos_sp_triplet; @@ -2583,7 +2673,11 @@ class MavlinkStreamPositionTargetGlobalInt : public MavlinkStream msg.alt = pos_sp_triplet.current.alt; mavlink_msg_position_target_global_int_send_struct(_mavlink->get_channel(), &msg); + + return true; } + + return false; } }; @@ -2635,7 +2729,7 @@ class MavlinkStreamLocalPositionSetpoint : public MavlinkStream _pos_sp_time(0) {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { struct vehicle_local_position_setpoint_s pos_sp; @@ -2656,7 +2750,11 @@ class MavlinkStreamLocalPositionSetpoint : public MavlinkStream msg.afz = pos_sp.acc_z; mavlink_msg_position_target_local_ned_send_struct(_mavlink->get_channel(), &msg); + + return true; } + + return false; } }; @@ -2712,7 +2810,7 @@ class MavlinkStreamAttitudeTarget : public MavlinkStream _att_rates_sp_time(0) {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { struct vehicle_attitude_setpoint_s att_sp = {}; @@ -2739,7 +2837,11 @@ class MavlinkStreamAttitudeTarget : public MavlinkStream msg.thrust = att_sp.thrust; mavlink_msg_attitude_target_send_struct(_mavlink->get_channel(), &msg); + + return true; } + + return false; } }; @@ -2791,7 +2893,7 @@ class MavlinkStreamRCChannels : public MavlinkStream _rc_time(0) {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { struct rc_input_values rc = {}; @@ -2840,7 +2942,11 @@ class MavlinkStreamRCChannels : public MavlinkStream over.chan8_raw = msg.chan8_raw; mavlink_msg_rc_channels_override_send_struct(_mavlink->get_channel(), &over); + + return true; } + + return false; } }; @@ -2892,7 +2998,7 @@ class MavlinkStreamManualControl : public MavlinkStream _manual_time(0) {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { struct manual_control_setpoint_s manual = {}; @@ -2914,7 +3020,11 @@ class MavlinkStreamManualControl : public MavlinkStream msg.buttons |= (manual.offboard_switch << (shift * 5)); mavlink_msg_manual_control_send_struct(_mavlink->get_channel(), &msg); + + return true; } + + return false; } }; @@ -2965,7 +3075,7 @@ class MavlinkStreamOpticalFlowRad : public MavlinkStream _flow_time(0) {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { struct optical_flow_s flow = {}; @@ -2987,7 +3097,11 @@ class MavlinkStreamOpticalFlowRad : public MavlinkStream msg.temperature = flow.gyro_temperature; mavlink_msg_optical_flow_rad_send_struct(_mavlink->get_channel(), &msg); + + return true; } + + return false; } }; @@ -3038,7 +3152,7 @@ class MavlinkStreamNamedValueFloat : public MavlinkStream _debug_time(0) {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { struct debug_key_value_s debug = {}; @@ -3052,7 +3166,11 @@ class MavlinkStreamNamedValueFloat : public MavlinkStream msg.value = debug.value; mavlink_msg_named_value_float_send_struct(_mavlink->get_channel(), &msg); + + return true; } + + return false; } }; @@ -3104,7 +3222,7 @@ class MavlinkStreamNavControllerOutput : public MavlinkStream _tecs_status_sub(_mavlink->add_orb_subscription(ORB_ID(tecs_status))) {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { struct fw_pos_ctrl_status_s _fw_pos_ctrl_status = {}; struct tecs_status_s _tecs_status = {}; @@ -3125,7 +3243,11 @@ class MavlinkStreamNavControllerOutput : public MavlinkStream msg.aspd_error = _tecs_status.airspeed_filtered - _tecs_status.airspeedSp; mavlink_msg_nav_controller_output_send_struct(_mavlink->get_channel(), &msg); + + return true; } + + return false; } }; @@ -3174,7 +3296,7 @@ class MavlinkStreamCameraCapture : public MavlinkStream _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))) {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { struct vehicle_status_s status = {}; (void)_status_sub->update(&status); @@ -3196,6 +3318,8 @@ class MavlinkStreamCameraCapture : public MavlinkStream msg.param7 = 0; mavlink_msg_command_long_send_struct(_mavlink->get_channel(), &msg); + + return true; } }; @@ -3246,7 +3370,7 @@ class MavlinkStreamDistanceSensor : public MavlinkStream _dist_sensor_time(0) {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { struct distance_sensor_s dist_sensor = {}; @@ -3284,7 +3408,11 @@ class MavlinkStreamDistanceSensor : public MavlinkStream msg.covariance = dist_sensor.covariance; mavlink_msg_distance_sensor_send_struct(_mavlink->get_channel(), &msg); + + return true; } + + return false; } }; @@ -3344,7 +3472,7 @@ class MavlinkStreamExtendedSysState : public MavlinkStream _msg.landed_state = MAV_LANDED_STATE_UNDEFINED; } - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { struct vehicle_status_s status = {}; struct vehicle_land_detected_s land_detected = {}; @@ -3400,6 +3528,8 @@ class MavlinkStreamExtendedSysState : public MavlinkStream if (updated) { mavlink_msg_extended_sys_state_send_struct(_mavlink->get_channel(), &_msg); } + + return updated; } }; @@ -3465,7 +3595,7 @@ class MavlinkStreamAltitude : public MavlinkStream _sensor_time(0) {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { mavlink_altitude_t msg = {}; bool updated = false; @@ -3538,6 +3668,8 @@ class MavlinkStreamAltitude : public MavlinkStream mavlink_msg_altitude_send_struct(_mavlink->get_channel(), &msg); } + + return updated; } }; @@ -3593,7 +3725,7 @@ class MavlinkStreamWind : public MavlinkStream _global_pos_time(0) {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { struct wind_estimate_s wind_estimate = {}; @@ -3621,7 +3753,11 @@ class MavlinkStreamWind : public MavlinkStream msg.vert_accuracy = 0.0f; mavlink_msg_wind_cov_send_struct(_mavlink->get_channel(), &msg); + + return true; } + + return false; } }; @@ -3672,7 +3808,7 @@ class MavlinkStreamMountOrientation : public MavlinkStream _mount_orientation_time(0) {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { struct mount_orientation_s mount_orientation = {}; @@ -3687,7 +3823,11 @@ class MavlinkStreamMountOrientation : public MavlinkStream msg.yaw = 180.0f / M_PI_F * mount_orientation.attitude_euler_angle[2]; mavlink_msg_mount_orientation_send_struct(_mavlink->get_channel(), &msg); + + return true; } + + return false; } }; @@ -3806,7 +3946,7 @@ class MavlinkStreamHighLatency : public MavlinkStream _tecs_time(0) {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { struct actuator_controls_s actuator = {}; struct airspeed_s airspeed = {}; @@ -3893,6 +4033,8 @@ class MavlinkStreamHighLatency : public MavlinkStream mavlink_msg_high_latency_send_struct(_mavlink->get_channel(), &msg); } + + return updated; } }; @@ -3953,7 +4095,7 @@ class MavlinkStreamGroundTruth : public MavlinkStream _gpos() {} - void send(const hrt_abstime t) + bool send(const hrt_abstime t) { bool att_updated = _att_sub->update(&_att_time, &_att); bool gpos_updated = _gpos_sub->update(&_gpos_time, &_gpos); @@ -3987,7 +4129,11 @@ class MavlinkStreamGroundTruth : public MavlinkStream } mavlink_msg_hil_state_quaternion_send_struct(_mavlink->get_channel(), &msg); + + return true; } + + return false; } }; diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index c4240f5306ae..bdcf23a692ca 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -93,6 +93,8 @@ class MavlinkOrbSubscription orb_id_t get_topic() const; int get_instance() const; + int get_fd() { return _fd; } + private: const orb_id_t _topic; ///< topic metadata int _fd; ///< subscription handle diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index ef958194b4f6..a530a69e0b54 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2084,7 +2084,7 @@ void MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg) t.flags = adsb.flags; t.squawk = adsb.squawk; - //warnx("code: %d callsign: %s, vel: %8.4f", (int)t.ICAO_address, t.callsign, (double)t.hor_velocity); + //PX4_INFO("code: %d callsign: %s, vel: %8.4f, tslc: %d", (int)t.ICAO_address, t.callsign, (double)t.hor_velocity, (int)t.tslc); if (_transponder_report_pub == nullptr) { _transponder_report_pub = orb_advertise_queue(ORB_ID(transponder_report), &t, transponder_report_s::ORB_QUEUE_LENGTH); diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 55bf14596412..82add6a35aef 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -78,7 +78,7 @@ MavlinkStream::update(const hrt_abstime t) // on the link scheduling _last_sent = hrt_absolute_time(); #ifndef __PX4_QURT - send(t); + (void)send(t); #endif return 0; } @@ -90,13 +90,13 @@ MavlinkStream::update(const hrt_abstime t) } int64_t dt = t - _last_sent; - int interval = _interval; + int interval = (_interval > 0) ? _interval : 0; if (!const_rate()) { interval /= _mavlink->get_rate_mult(); } - // send the message if it is due or + // Send the message if it is due or // if it will overrun the next scheduled send interval // by 40% of the interval time. This helps to avoid // sending a scheduled message on average slower than @@ -106,17 +106,23 @@ MavlinkStream::update(const hrt_abstime t) // This method is not theoretically optimal but a suitable // stopgap as it hits its deadlines well (0.5 Hz, 50 Hz and 250 Hz) - if (dt > (interval - (_mavlink->get_main_loop_delay() / 10) * 4)) { + if (interval == 0 || (dt > (interval - (_mavlink->get_main_loop_delay() / 10) * 4))) { // interval expired, send message + bool sent = true; #ifndef __PX4_QURT - send(t); + sent = send(t); #endif - // if the interval is non-zero do not use the actual time but + + // If the interval is non-zero do not use the actual time but // increment at a fixed rate, so that processing delays do not // distort the average rate - _last_sent = (interval > 0) ? _last_sent + interval : t; + if (sent) { + _last_sent = (interval > 0) ? _last_sent + interval : t; + return 0; - return 0; + } else { + return -1; + } } return -1; diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 8b3ec4617ffc..28f2269c70be 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -100,7 +100,7 @@ class MavlinkStream unsigned int _interval; ///< if set to zero = unlimited rate #ifndef __PX4_QURT - virtual void send(const hrt_abstime t) = 0; + virtual bool send(const hrt_abstime t) = 0; #endif private: