From ef46acda711879804b511eb6afec3919e8228809 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 21 May 2018 12:18:57 +1000 Subject: [PATCH] GCS_MAVLink: support MAV_CMD_SET_MESSAGE_INTERVAL --- libraries/GCS_MAVLink/GCS.h | 127 ++- libraries/GCS_MAVLink/GCS_Common.cpp | 763 +++++++++++++++--- libraries/GCS_MAVLink/GCS_Dummy.h | 2 + libraries/GCS_MAVLink/GCS_Param.cpp | 94 +-- .../GCS_MAVLink/examples/routing/routing.cpp | 2 + 5 files changed, 792 insertions(+), 196 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 62a08e6948..609dab4458 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -15,7 +15,6 @@ #include #include #include -#include #include #include #include @@ -23,6 +22,9 @@ #include #include #include +#include + +#define DEBUG_SEND_MESSAGE_TIMINGS 0 // check if a message will fit in the payload space available #define PAYLOAD_SIZE(chan, id) (GCS_MAVLINK::packet_overhead_chan(chan)+MAVLINK_MSG_ID_ ## id ## _LEN) @@ -105,13 +107,13 @@ class GCS_MAVLINK public: friend class GCS; GCS_MAVLINK(); - void update(uint32_t max_time_us=1000); + void update_receive(uint32_t max_time_us=1000); + void update_send(); void init(AP_HAL::UARTDriver *port, mavlink_channel_t mav_chan); void setup_uart(const AP_SerialManager& serial_manager, AP_SerialManager::SerialProtocol protocol, uint8_t instance); void send_message(enum ap_message id); void send_text(MAV_SEVERITY severity, const char *fmt, ...); void send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list); - void data_stream_send(); void queued_param_send(); void queued_waypoint_send(); // packetReceived is called on any successful decode of a mavlink message @@ -145,9 +147,6 @@ public: NUM_STREAMS }; - // see if we should send a stream now. Called at 50Hz - bool stream_trigger(enum streams stream_num); - bool is_high_bandwidth() { return chan == MAVLINK_COMM_0; } // return true if this channel has hardware flow control bool have_flow_control(); @@ -230,9 +229,6 @@ public: // send queued parameters if needed void send_queued_parameters(void); - // push send_message() messages and queued statustext messages etc: - void retry_deferred(); - /* send a MAVLink message to all components with this vehicle's system id This is a no-op if no routes to components have been learned @@ -337,6 +333,10 @@ protected: virtual bool should_zero_rc_outputs_on_reboot() const { return false; } MAV_RESULT handle_preflight_reboot(const mavlink_command_long_t &packet); void disable_overrides(); + + // reset a message interval via mavlink: + MAV_RESULT handle_command_set_message_interval(const mavlink_command_long_t &packet); + MAV_RESULT handle_rc_bind(const mavlink_command_long_t &packet); virtual MAV_RESULT handle_flight_termination(const mavlink_command_long_t &packet); @@ -414,8 +414,6 @@ protected: private: - float adjust_rate_for_stream_trigger(enum streams stream_num); - MAV_RESULT _set_mode_common(const MAV_MODE base_mode, const uint32_t custom_mode); virtual void handleMessage(mavlink_message_t * msg) = 0; @@ -459,10 +457,7 @@ private: uint32_t waypoint_timelast_request; // milliseconds const uint16_t waypoint_receive_timeout = 8000; // milliseconds - // number of 50Hz ticks until we next send this stream - uint8_t stream_ticks[NUM_STREAMS]; - - // number of extra ticks to add to slow things down for the radio + // number of extra 20ms intervals to add to slow things down for the radio uint8_t stream_slowdown; // perf counters @@ -471,11 +466,81 @@ private: char _perf_packet_name[16]; char _perf_update_name[16]; - // deferred message handling. We size the deferred_message - // ringbuffer so we can defer every message type - enum ap_message deferred_messages[MSG_LAST]; - uint8_t next_deferred_message; - uint8_t num_deferred_messages; + // outbound ("deferred message") queue. + + // "special" messages such as heartbeat, next_param etc are + // stored separately to stream-rated messages like AHRS2 etc: + struct deferred_message_t { + const ap_message id; + uint16_t interval_ms; + uint16_t last_sent_ms; // from AP_HAL::millis16() + } deferred_message[2] = { + { MSG_HEARTBEAT, }, + { MSG_NEXT_PARAM, }, + }; + // returns index of id in deferred_message[] or -1 if not present + int8_t get_deferred_message_index(const ap_message id) const; + // returns index of a message in deferred_message[] which should + // be sent (or -1 if none to send at the moment) + int8_t deferred_message_to_send_index(); + // cache of which deferred message should be sent next: + int8_t next_deferred_message_to_send_cache = -1; + + struct deferred_message_bucket_t { + Bitmask ap_message_ids{MSG_LAST}; + uint16_t interval_ms; + uint16_t last_sent_ms; // from AP_HAL::millis16() + }; + deferred_message_bucket_t deferred_message_bucket[5]; + static const uint8_t no_bucket_to_send = -1; + static const ap_message no_message_to_send = (ap_message)-1; + uint8_t sending_bucket_id = no_bucket_to_send; + Bitmask bucket_message_ids_to_send{MSG_LAST}; + + ap_message next_deferred_message_to_send(); + void find_next_bucket_to_send(); + void remove_message_from_bucket(int8_t bucket, ap_message id); + + // bitmask of IDs the code has spontaneously decided it wants to + // send out. Examples include HEARTBEAT (gcs_send_heartbeat) + Bitmask pushed_ap_message_ids{MSG_LAST}; + + // returns true if it is OK to send a message while we are in + // delay callback. In particular, when we are doing sensor init + // we still send heartbeats. + bool should_send_message_in_delay_callback(const ap_message id) const; + + // if true is returned, interval will contain the default interval for id + bool get_default_interval_for_ap_message(const ap_message id, uint16_t &interval) const; + // if true is returned, interval will contain the default interval for id + bool get_default_interval_for_mavlink_message_id(const uint32_t mavlink_message_id, uint16_t &interval) const; + // returns an interval in milliseconds for any ap_message in stream id + uint16_t get_interval_for_stream(GCS_MAVLINK::streams id) const; + // set an inverval for a specific mavlink message. Returns false + // on failure (typically because there is no mapping from that + // mavlink ID to an ap_message) + bool set_mavlink_message_id_interval(const uint32_t mavlink_id, + const uint16_t interval_ms); + // map a mavlink ID to an ap_message which, if passed to + // try_send_message, will cause a mavlink message with that id to + // be emitted. Returns MSG_LAST if no such mapping exists. + ap_message mavlink_id_to_ap_message_id(const uint32_t mavlink_id) const; + // set the interval at which an ap_message should be emitted (in ms) + bool set_ap_message_interval(enum ap_message id, uint16_t interval_ms); + // call set_ap_message_interval for each entry in a stream, + // the interval being based on the stream's rate + void initialise_message_intervals_for_stream(GCS_MAVLINK::streams id); + // call initialise_message_intervals_for_stream on every stream: + void initialise_message_intervals_from_streamrates(); + // boolean that indicated that message intervals have been set + // from streamrates: + bool deferred_messages_initialised; + // return interval deferred message bucket should be sent after. + // When sending parameters and waypoints this may be longer than + // the interval specified in "deferred" + uint16_t get_reschedule_interval_ms(const deferred_message_bucket_t &deferred) const; + + bool do_try_send_message(const ap_message id); // time when we missed sending a parameter for GCS static uint32_t reserve_param_space_start_ms; @@ -550,7 +615,6 @@ private: const float roll, const float pitch, const float yaw); - void push_deferred_messages(); void lock_channel(mavlink_channel_t chan, bool lock); @@ -588,6 +652,21 @@ private: void send_global_position_int(); void zero_rc_outputs(); + +#if DEBUG_SEND_MESSAGE_TIMINGS + struct { + uint32_t longest_time_us; + ap_message longest_id; + uint32_t no_space_for_message; + uint16_t statustext_last_sent_ms; + uint32_t behind; + uint16_t fnbts_maxtime; + uint32_t max_retry_deferred_body_us; + uint8_t max_retry_deferred_body_type; + } try_send_message_stats; + uint8_t max_slowdown; +#endif + }; /// @class GCS @@ -630,10 +709,8 @@ public: ap_var_type param_type, float param_value); - // push send_message() messages and queued statustext messages etc: - void retry_deferred(); - void data_stream_send(); - void update(); + void update_send(); + void update_receive(); virtual void setup_uarts(AP_SerialManager &serial_manager); bool out_of_time() const { diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 37bd8b5ee4..ad723a7e4c 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include "GCS.h" @@ -687,6 +688,12 @@ void GCS_MAVLINK::handle_radio_status(mavlink_message_t *msg, DataFlash_Class &d stream_slowdown--; } +#if DEBUG_SEND_MESSAGE_TIMINGS + if (stream_slowdown > max_slowdown) { + max_slowdown = stream_slowdown; + } +#endif + //log rssi, noise, etc if logging Performance monitoring data if (log_radio) { dataflash.Log_Write_Radio(packet); @@ -835,67 +842,523 @@ mission_ack: return mission_is_complete; } -void GCS_MAVLINK::push_deferred_messages() +ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) const { - while (num_deferred_messages != 0) { - if (!try_send_message(deferred_messages[next_deferred_message])) { + // MSG_NEXT_WAYPOINT doesn't correspond to a mavlink message directly. + // It is used to request the next waypoint after receiving one. + + // MSG_NEXT_PARAM doesn't correspond to a mavlink message directly. + // It is used to send the next parameter in a stream after sending one + + // MSG_NAMED_FLOAT messages can't really be "streamed"... + + // this list is ordered by AP_MESSAGE ID - the value being returned: + static const struct { + uint32_t mavlink_id; + ap_message msg_id; + } map[] { + { MAVLINK_MSG_ID_HEARTBEAT, MSG_HEARTBEAT}, + { MAVLINK_MSG_ID_ATTITUDE, MSG_ATTITUDE}, + { MAVLINK_MSG_ID_GLOBAL_POSITION_INT, MSG_LOCATION}, + { MAVLINK_MSG_ID_SYS_STATUS, MSG_EXTENDED_STATUS1}, + { MAVLINK_MSG_ID_MEMINFO, MSG_MEMINFO}, + { MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, MSG_NAV_CONTROLLER_OUTPUT}, + { MAVLINK_MSG_ID_MISSION_CURRENT, MSG_CURRENT_WAYPOINT}, + { MAVLINK_MSG_ID_VFR_HUD, MSG_VFR_HUD}, + { MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, MSG_SERVO_OUTPUT_RAW}, + { MAVLINK_MSG_ID_RC_CHANNELS, MSG_RADIO_IN}, + { MAVLINK_MSG_ID_RAW_IMU, MSG_RAW_IMU1}, + { MAVLINK_MSG_ID_SCALED_PRESSURE, MSG_SCALED_PRESSURE}, + { MAVLINK_MSG_ID_SENSOR_OFFSETS, MSG_SENSOR_OFFSETS}, + { MAVLINK_MSG_ID_GPS_RAW_INT, MSG_GPS_RAW}, + { MAVLINK_MSG_ID_GPS_RTK, MSG_GPS_RTK}, + { MAVLINK_MSG_ID_GPS2_RAW, MSG_GPS2_RAW}, + { MAVLINK_MSG_ID_GPS2_RTK, MSG_GPS2_RTK}, + { MAVLINK_MSG_ID_SYSTEM_TIME, MSG_SYSTEM_TIME}, + { MAVLINK_MSG_ID_RC_CHANNELS_SCALED, MSG_SERVO_OUT}, + { MAVLINK_MSG_ID_PARAM_VALUE, MSG_NEXT_PARAM}, + { MAVLINK_MSG_ID_FENCE_STATUS, MSG_FENCE_STATUS}, + { MAVLINK_MSG_ID_AHRS, MSG_AHRS}, + { MAVLINK_MSG_ID_SIMSTATE, MSG_SIMSTATE}, + { MAVLINK_MSG_ID_HWSTATUS, MSG_HWSTATUS}, + { MAVLINK_MSG_ID_WIND, MSG_WIND}, + { MAVLINK_MSG_ID_RANGEFINDER, MSG_RANGEFINDER}, + // request also does report: + { MAVLINK_MSG_ID_TERRAIN_REQUEST, MSG_TERRAIN}, + { MAVLINK_MSG_ID_BATTERY2, MSG_BATTERY2}, + { MAVLINK_MSG_ID_CAMERA_FEEDBACK, MSG_CAMERA_FEEDBACK}, + { MAVLINK_MSG_ID_MOUNT_STATUS, MSG_MOUNT_STATUS}, + { MAVLINK_MSG_ID_OPTICAL_FLOW, MSG_OPTICAL_FLOW}, + { MAVLINK_MSG_ID_GIMBAL_REPORT, MSG_GIMBAL_REPORT}, + { MAVLINK_MSG_ID_MAG_CAL_PROGRESS, MSG_MAG_CAL_PROGRESS}, + { MAVLINK_MSG_ID_MAG_CAL_REPORT, MSG_MAG_CAL_REPORT}, + { MAVLINK_MSG_ID_EKF_STATUS_REPORT, MSG_EKF_STATUS_REPORT}, + { MAVLINK_MSG_ID_LOCAL_POSITION_NED, MSG_LOCAL_POSITION}, + { MAVLINK_MSG_ID_PID_TUNING, MSG_PID_TUNING}, + { MAVLINK_MSG_ID_VIBRATION, MSG_VIBRATION}, + { MAVLINK_MSG_ID_RPM, MSG_RPM}, + { MAVLINK_MSG_ID_MISSION_ITEM_REACHED, MSG_MISSION_ITEM_REACHED}, + { MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT, MSG_POSITION_TARGET_GLOBAL_INT}, + { MAVLINK_MSG_ID_ADSB_VEHICLE, MSG_ADSB_VEHICLE}, + { MAVLINK_MSG_ID_BATTERY_STATUS, MSG_BATTERY_STATUS}, + { MAVLINK_MSG_ID_AOA_SSA, MSG_AOA_SSA}, + { MAVLINK_MSG_ID_DEEPSTALL, MSG_LANDING}, + }; + + for (uint8_t i=0; i 60000) { + return 60000; + } + + return interval_ms; +} + +// typical runtime on fmuv3: 5 microseconds for 3 buckets +void GCS_MAVLINK::find_next_bucket_to_send() +{ +#if DEBUG_SEND_MESSAGE_TIMINGS + void *data = hal.scheduler->disable_interrupts_save(); + uint32_t start_us = AP_HAL::micros(); +#endif + + const uint16_t now16_ms{AP_HAL::millis16()}; + + // all done sending this bucket... find another bucket... + sending_bucket_id = no_bucket_to_send; + uint16_t ms_before_send_next_bucket_to_send = UINT16_MAX; + for (uint8_t i=0; i interval) { + // should already have sent this bucket! + ms_before_send_this_bucket = 0; + } else { + ms_before_send_this_bucket = interval - ms_since_last_sent; + } + if (sending_bucket_id == no_bucket_to_send || + ms_before_send_this_bucket < ms_before_send_next_bucket_to_send) { + sending_bucket_id = i; + ms_before_send_next_bucket_to_send = ms_before_send_this_bucket; + } + } + if (sending_bucket_id != no_bucket_to_send) { + bucket_message_ids_to_send = deferred_message_bucket[sending_bucket_id].ap_message_ids; + } else { + bucket_message_ids_to_send.clearall(); + } + +#if DEBUG_SEND_MESSAGE_TIMINGS + uint32_t delta_us = AP_HAL::micros() - start_us; + hal.scheduler->restore_interrupts(data); + if (delta_us > try_send_message_stats.fnbts_maxtime) { + try_send_message_stats.fnbts_maxtime = delta_us; + } +#endif +} + +ap_message GCS_MAVLINK::next_deferred_message_to_send() +{ + if (sending_bucket_id == no_bucket_to_send) { + // could happen if all streamrates are zero? + return no_message_to_send; + } + + const uint16_t now16_ms = AP_HAL::millis16(); + const uint16_t ms_since_last_sent = now16_ms - deferred_message_bucket[sending_bucket_id].last_sent_ms; + if (ms_since_last_sent < deferred_message_bucket[sending_bucket_id].interval_ms) { + // not time to send this bucket + return no_message_to_send; + } + + const int16_t next = bucket_message_ids_to_send.first_set(); + if (next == -1) { + // should not happen +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + AP_HAL::panic("next_deferred_message_to_send called on empty bucket"); +#endif + find_next_bucket_to_send(); + } + return (ap_message)next; +} + +// call try_send_message if appropriate. Incorporates code to record +// how long it takes to send a message. +// try_send_message is expected to be overridden, not this function. +bool GCS_MAVLINK::do_try_send_message(const ap_message id) +{ + const bool in_delay_callback = hal.scheduler->in_delay_callback(); + if (in_delay_callback && !should_send_message_in_delay_callback(id)) { + return true; + } +#if DEBUG_SEND_MESSAGE_TIMINGS + void *data = hal.scheduler->disable_interrupts_save(); + uint32_t start_send_message_us = AP_HAL::micros(); +#endif + if (!try_send_message(id)) { + // didn't fit in buffer... +#if DEBUG_SEND_MESSAGE_TIMINGS + try_send_message_stats.no_space_for_message++; + hal.scheduler->restore_interrupts(data); +#endif + return false; + } +#if DEBUG_SEND_MESSAGE_TIMINGS + const uint32_t delta_us = AP_HAL::micros() - start_send_message_us; + hal.scheduler->restore_interrupts(data); + if (delta_us > try_send_message_stats.longest_time_us) { + try_send_message_stats.longest_time_us = delta_us; + try_send_message_stats.longest_id = id; + } +#endif + return true; +} + +int8_t GCS_MAVLINK::get_deferred_message_index(const ap_message id) const +{ + for (uint8_t i=0; i interval_ms) { + // should already have sent this one! + ms_before_send_this_message = 0; + } else { + ms_before_send_this_message = interval_ms - ms_since_last_sent; + } + if (ms_before_send_this_message < ms_before_next_message_to_send) { + next_deferred_message_to_send_cache = i; + ms_before_next_message_to_send = ms_before_send_this_message; + } + } + } + + if (next_deferred_message_to_send_cache == -1) { + // this really shouldn't happen; we force parameter rates, for example. + return -1; + } + + const uint16_t ms_since_last_sent = now16_ms - deferred_message[next_deferred_message_to_send_cache].last_sent_ms; + if (ms_since_last_sent < deferred_message[next_deferred_message_to_send_cache].interval_ms) { + return -1; + } + + return next_deferred_message_to_send_cache; +} + +void GCS_MAVLINK::update_send() +{ + if (!hal.scheduler->in_delay_callback()) { + // DataFlash_Class will not send log data if we are armed. + DataFlash_Class::instance()->handle_log_send(); + } + + if (!deferred_messages_initialised) { + initialise_message_intervals_from_streamrates(); + deferred_messages_initialised = true; + } + +#if DEBUG_SEND_MESSAGE_TIMINGS + uint32_t retry_deferred_body_start = 0; +#endif + + const uint32_t start = AP_HAL::millis(); + gcs().set_out_of_time(false); + while (AP_HAL::millis() - start < 5) { // spend a max of 5ms sending messages. This should never trigger - out_of_time() should become true + if (gcs().out_of_time()) { break; } - next_deferred_message++; - if (next_deferred_message == ARRAY_SIZE(deferred_messages)) { - next_deferred_message = 0; + +#if DEBUG_SEND_MESSAGE_TIMINGS + retry_deferred_body_start = AP_HAL::micros(); +#endif + + // check if any "specially handled" messages should be sent out + { + const int8_t next = deferred_message_to_send_index(); + if (next != -1) { + if (!do_try_send_message(deferred_message[next].id)) { + break; + } + deferred_message[next].last_sent_ms += deferred_message[next].interval_ms; + next_deferred_message_to_send_cache = -1; // deferred_message_to_send will recalculate +#if DEBUG_SEND_MESSAGE_TIMINGS + const uint32_t stop = AP_HAL::micros(); + const uint32_t delta = stop - retry_deferred_body_start; + if (delta > try_send_message_stats.max_retry_deferred_body_us) { + try_send_message_stats.max_retry_deferred_body_us = delta; + try_send_message_stats.max_retry_deferred_body_type = 1; + } +#endif + continue; + } + } + + // check for any messages that the code has explicity sent + ap_message next = (ap_message)pushed_ap_message_ids.first_set(); + if (next != no_message_to_send) { + if (!do_try_send_message(next)) { + break; + } + pushed_ap_message_ids.clear(next); +#if DEBUG_SEND_MESSAGE_TIMINGS + const uint32_t stop = AP_HAL::micros(); + const uint32_t delta = stop - retry_deferred_body_start; + if (delta > try_send_message_stats.max_retry_deferred_body_us) { + try_send_message_stats.max_retry_deferred_body_us = delta; + try_send_message_stats.max_retry_deferred_body_type = 2; + } +#endif + continue; + } + + next = next_deferred_message_to_send(); + if (next != no_message_to_send) { + if (!do_try_send_message(next)) { + break; + } + bucket_message_ids_to_send.clear(next); + if (bucket_message_ids_to_send.first_set() == -1) { + // we sent everything in the bucket. Reschedule it. + deferred_message_bucket[sending_bucket_id].last_sent_ms += + deferred_message_bucket[sending_bucket_id].interval_ms; + find_next_bucket_to_send(); + } +#if DEBUG_SEND_MESSAGE_TIMINGS + const uint32_t stop = AP_HAL::micros(); + const uint32_t delta = stop - retry_deferred_body_start; + if (delta > try_send_message_stats.max_retry_deferred_body_us) { + try_send_message_stats.max_retry_deferred_body_us = delta; + try_send_message_stats.max_retry_deferred_body_type = 3; + } +#endif + continue; + } + break; + } +#if DEBUG_SEND_MESSAGE_TIMINGS + const uint32_t stop = AP_HAL::micros(); + const uint32_t delta = stop - retry_deferred_body_start; + if (delta > try_send_message_stats.max_retry_deferred_body_us) { + try_send_message_stats.max_retry_deferred_body_us = delta; + try_send_message_stats.max_retry_deferred_body_type = 4; + } +#endif +} + +void GCS_MAVLINK::remove_message_from_bucket(int8_t bucket, ap_message id) +{ + deferred_message_bucket[bucket].ap_message_ids.clear(id); + if (deferred_message_bucket[bucket].ap_message_ids.first_set() == -1) { + // bucket empty. Free it: + deferred_message_bucket[bucket].interval_ms = 0; + deferred_message_bucket[bucket].last_sent_ms = 0; + if (sending_bucket_id == bucket) { + find_next_bucket_to_send(); } - num_deferred_messages--; } } -void GCS_MAVLINK::retry_deferred() +bool GCS_MAVLINK::set_ap_message_interval(enum ap_message id, uint16_t interval_ms) { - push_deferred_messages(); + if (id == MSG_NEXT_PARAM) { + // force parameters to *always* get streamed so a vehicle is + // recoverable from bad configuration: + if (interval_ms == 0) { + interval_ms = 100; + } else if (interval_ms > 1000) { + interval_ms = 1000; + } + } + + // send messages out at most once per main loop iteration: + if (interval_ms != 0 && + interval_ms*1000 < AP::scheduler().get_loop_period_us()) { + interval_ms = AP::scheduler().get_loop_period_us()/1000; + } + + // check if it's a specially-handled message: + const int8_t deferred_offset = get_deferred_message_index(id); + if (deferred_offset != -1) { + deferred_message[deferred_offset].interval_ms = interval_ms; + deferred_message[deferred_offset].last_sent_ms = AP_HAL::millis16(); + return true; + } + + // see which bucket has the closest interval: + int8_t closest_bucket = -1; + uint16_t closest_bucket_interval_delta = UINT16_MAX; + int8_t in_bucket = -1; + int8_t empty_bucket_id = -1; + for (uint8_t i=0; i 5 && + empty_bucket_id != -1) { + // allocate a bucket for this interval + deferred_message_bucket[empty_bucket_id].interval_ms = interval_ms; + deferred_message_bucket[empty_bucket_id].last_sent_ms = AP_HAL::millis16(); + closest_bucket = empty_bucket_id; + closest_bucket_interval_delta = 0; + } else if (deferred_message_bucket[closest_bucket].ap_message_ids.count() == 0) { + deferred_message_bucket[closest_bucket].interval_ms = interval_ms; + } + + deferred_message_bucket[closest_bucket].ap_message_ids.set(id); + + if (sending_bucket_id == no_bucket_to_send) { + sending_bucket_id = closest_bucket; + bucket_message_ids_to_send = deferred_message_bucket[closest_bucket].ap_message_ids; + } + + return true; } -// send a message using mavlink, handling message queueing +// queue a message to be sent (try_send_message does the *actual* +// mavlink work!) void GCS_MAVLINK::send_message(enum ap_message id) { - uint8_t i, nextid; - if (id == MSG_HEARTBEAT) { save_signing_timestamp(false); } - // see if we can send the deferred messages, if any: - push_deferred_messages(); - - // if there are no deferred messages, attempt to send straight away: - if (num_deferred_messages == 0) { - if (try_send_message(id)) { - // yay, we sent it! - return; - } - } - - // we failed to send the message this time around, so try to defer: - if (num_deferred_messages == ARRAY_SIZE(deferred_messages)) { - // the defer buffer is full, discard this attempt to send. - // Note that the message *may* already be in the defer buffer - return; - } - - // check if this message is deferred: - for (i=0, nextid = next_deferred_message; i < num_deferred_messages; i++) { - if (deferred_messages[nextid] == id) { - // it's already deferred - return; - } - nextid++; - if (nextid == ARRAY_SIZE(deferred_messages)) { - nextid = 0; - } - } - - // not already deferred, defer it - deferred_messages[nextid] = id; - num_deferred_messages++; + pushed_ap_message_ids.set(id); } void GCS_MAVLINK::packetReceived(const mavlink_status_t &status, @@ -928,7 +1391,7 @@ void GCS_MAVLINK::packetReceived(const mavlink_status_t &status, } void -GCS_MAVLINK::update(uint32_t max_time_us) +GCS_MAVLINK::update_receive(uint32_t max_time_us) { // receive new packets mavlink_message_t msg; @@ -999,6 +1462,73 @@ GCS_MAVLINK::update(uint32_t max_time_us) } } +#if DEBUG_SEND_MESSAGE_TIMINGS + + const uint16_t now16_ms{AP_HAL::millis16()}; + + if (uint16_t(now16_ms - try_send_message_stats.statustext_last_sent_ms) > 10000U) { + if (try_send_message_stats.longest_time_us) { + gcs().send_text(MAV_SEVERITY_INFO, + "GCS.chan(%u): ap_msg=%u took %uus to send", + chan, + try_send_message_stats.longest_id, + try_send_message_stats.longest_time_us); + try_send_message_stats.longest_time_us = 0; + } + if (try_send_message_stats.no_space_for_message && + (is_active() || is_streaming())) { + gcs().send_text(MAV_SEVERITY_INFO, + "GCS.chan(%u): out-of-space: %u", + chan, + try_send_message_stats.no_space_for_message); + try_send_message_stats.no_space_for_message = 0; + } + if (max_slowdown) { + gcs().send_text(MAV_SEVERITY_INFO, + "GCS.chan(%u): max slowdown=%u", + chan, + max_slowdown); + max_slowdown = 0; + } + if (try_send_message_stats.behind) { + gcs().send_text(MAV_SEVERITY_INFO, + "GCS.chan(%u): behind=%u", + chan, + try_send_message_stats.behind); + try_send_message_stats.behind = 0; + } + if (try_send_message_stats.fnbts_maxtime) { + gcs().send_text(MAV_SEVERITY_INFO, + "GCS.chan(%u): fnbts_maxtime=%uus", + chan, + try_send_message_stats.fnbts_maxtime); + try_send_message_stats.fnbts_maxtime = 0; + } + if (try_send_message_stats.max_retry_deferred_body_us) { + gcs().send_text(MAV_SEVERITY_INFO, + "GCS.chan(%u): retry_body_maxtime=%uus (%u)", + chan, + try_send_message_stats.max_retry_deferred_body_us, + try_send_message_stats.max_retry_deferred_body_type + ); + try_send_message_stats.max_retry_deferred_body_us = 0; + } + + for (uint8_t i=0; i 60000000) { + interval_ms = 60000; + } else { + interval_ms = interval_us / 1000; + } + if (set_mavlink_message_id_interval(msg_id, interval_ms)) { + return MAV_RESULT_ACCEPTED; } - return 1.0f; + return MAV_RESULT_FAILED; } // are we still delaying telemetry to try to avoid Xbee bricking? @@ -2852,6 +3390,10 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t } break; + case MAV_CMD_SET_MESSAGE_INTERVAL: + result = handle_command_set_message_interval(packet); + break; + case MAV_CMD_DO_SET_SERVO: case MAV_CMD_DO_REPEAT_SERVO: case MAV_CMD_DO_SET_RELAY: @@ -3298,55 +3840,66 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) return ret; } -void GCS_MAVLINK::data_stream_send(void) +uint16_t GCS_MAVLINK::get_interval_for_stream(GCS_MAVLINK::streams id) const { - if (waypoint_receiving) { - // don't interfere with mission transfer - return; + const float frate = streamRates[id].get(); + if ((uint8_t)frate == 0) { + return 0; } - - if (!hal.scheduler->in_delay_callback()) { - // DataFlash_Class will not send log data if we are armed. - DataFlash_Class::instance()->handle_log_send(); - } - - gcs().set_out_of_time(false); - - send_queued_parameters(); - - if (gcs().out_of_time()) return; - - if (hal.scheduler->in_delay_callback()) { - if (in_hil_mode()) { - // in HIL we need to keep sending servo values to ensure - // the simulator doesn't pause, otherwise our sensor - // calibration could stall - if (stream_trigger(STREAM_RAW_CONTROLLER)) { - send_message(MSG_SERVO_OUT); - } - if (stream_trigger(STREAM_RC_CHANNELS)) { - send_message(MSG_SERVO_OUTPUT_RAW); - } - } - // send no other streams while in delay, just in case they - // take way too long to run - return; + const uint32_t ret = 1000/(uint8_t)frate; + if (ret > 60000) { + return 60000; } + return ret; +} +void GCS_MAVLINK::initialise_message_intervals_for_stream(GCS_MAVLINK::streams id) +{ for (uint8_t i=0; all_stream_entries[i].ap_message_ids != nullptr; i++) { - const streams id = (streams)all_stream_entries[i].stream_id; - if (!stream_trigger(id)) { + const GCS_MAVLINK::stream_entries &entries = all_stream_entries[i]; + if (entries.stream_id != id) { continue; } - const ap_message *msg_ids = all_stream_entries[i].ap_message_ids; - for (uint8_t j=0; jset_and_save_ifchanged(freq); } else { rate->set(freq); } + initialise_message_intervals_for_stream(stream_id); } } @@ -239,6 +250,12 @@ void GCS_MAVLINK::handle_param_request_read(mavlink_message_t *msg) // queue it for processing by io timer param_requests.push(req); + + // speaking of which, we'd best make sure it is running: + if (!param_timer_registered) { + param_timer_registered = true; + hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&GCS_MAVLINK::param_io_timer, void)); + } } void GCS_MAVLINK::handle_param_set(mavlink_message_t *msg) @@ -281,50 +298,6 @@ void GCS_MAVLINK::handle_param_set(mavlink_message_t *msg) } } -// see if we should send a stream now. Called at 50Hz -bool GCS_MAVLINK::stream_trigger(enum streams stream_num) -{ - if (stream_num >= NUM_STREAMS) { - return false; - } - float rate = (uint8_t)streamRates[stream_num].get(); - - rate *= adjust_rate_for_stream_trigger(stream_num); - - if (rate <= 0) { - if (chan_is_streaming & (1U<<(chan-MAVLINK_COMM_0))) { - // if currently streaming then check if all streams are disabled - // to allow runtime detection of user disabling streaming - bool is_streaming = false; - for (uint8_t i=0; i 0) { - is_streaming = true; - } - } - if (!is_streaming) { - // all streams have been turned off, clear the bit flag - chan_is_streaming &= ~(1U<<(chan-MAVLINK_COMM_0)); - } - } - return false; - } else { - chan_is_streaming |= (1U<<(chan-MAVLINK_COMM_0)); - } - - if (stream_ticks[stream_num] == 0) { - // we're triggering now, setup the next trigger point - if (rate > 50) { - rate = 50; - } - stream_ticks[stream_num] = (50 / rate) - 1 + stream_slowdown; - return true; - } - - // count down at 50Hz - stream_ticks[stream_num]--; - return false; -} - /* send a parameter value message to all active MAVLink connections */ @@ -361,17 +334,6 @@ void GCS_MAVLINK::send_queued_parameters(void) param_timer_registered = true; hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&GCS_MAVLINK::param_io_timer, void)); } - - if (_queued_parameter == nullptr && - param_replies.empty()) { - return; - } - if (streamRates[STREAM_PARAMS].get() <= 0) { - streamRates[STREAM_PARAMS].set(10); - } - if (stream_trigger(STREAM_PARAMS)) { - send_message(MSG_NEXT_PARAM); - } } diff --git a/libraries/GCS_MAVLink/examples/routing/routing.cpp b/libraries/GCS_MAVLink/examples/routing/routing.cpp index ea4392c734..32fee85321 100644 --- a/libraries/GCS_MAVLink/examples/routing/routing.cpp +++ b/libraries/GCS_MAVLink/examples/routing/routing.cpp @@ -20,6 +20,8 @@ const AP_FWVersion AP_FWVersion::fwver fw_string: "routing example" }; +const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] {}; + class GCS_MAVLINK_routing : public GCS_MAVLINK {