Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: Check if set_message_interval is too fast

This commit is contained in:
Stephen Dade 2024-09-18 20:34:04 +10:00 committed by Peter Barker
parent 1a15e50434
commit 9a563e222f
2 changed files with 24 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/GCS_MAVLink/GCS.h
View File

@ -267,6 +267,9 @@ public:
// accessor for uart
AP_HAL::UARTDriver *get_uart() { return _port; }
// cap the MAVLink message rate. It can't be greater than 0.8 * SCHED_LOOP_RATE
uint16_t cap_message_interval(uint16_t interval_ms) const;
virtual uint8_t sysid_my_gcs() const = 0;
virtual bool sysid_enforce() const { return false; }

26
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -225,6 +225,20 @@ void GCS_MAVLINK::send_power_status(void)
hal.analogin->power_status_flags());
}
#if AP_SCHEDULER_ENABLED
// cap the MAVLink message rate. It can't be greater than 0.8 * SCHED_LOOP_RATE
uint16_t GCS_MAVLINK::cap_message_interval(uint16_t interval_ms) const
{
if (interval_ms == 0) {
return 0;
}
if (interval_ms*800 < AP::scheduler().get_loop_period_us()) {
return AP::scheduler().get_loop_period_us()/800.0f;
}
return interval_ms;
}
#endif
#if HAL_WITH_MCU_MONITORING
// report MCU voltage/temperature status
void GCS_MAVLINK::send_mcu_status(void)
@ -1657,11 +1671,7 @@ bool GCS_MAVLINK::set_ap_message_interval(enum ap_message id, uint16_t interval_
}
#if AP_SCHEDULER_ENABLED
// send messages out at most 80% of main loop rate
if (interval_ms != 0 &&
interval_ms*800 < AP::scheduler().get_loop_period_us()) {
interval_ms = AP::scheduler().get_loop_period_us()/800.0f;
}
interval_ms = cap_message_interval(interval_ms);
#endif
// check if it's a specially-handled message:
@ -3143,6 +3153,12 @@ MAV_RESULT GCS_MAVLINK::set_message_interval(uint32_t msg_id, int32_t interval_u
} else {
interval_ms = interval_us / 1000;
}
#if AP_SCHEDULER_ENABLED
if (interval_ms != 0 && cap_message_interval(interval_ms) > interval_ms) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Requested rate for message ID %u too fast. Increase SCHED_LOOP_RATE", (unsigned int)msg_id);
return MAV_RESULT_DENIED;
}
#endif
if (set_ap_message_interval(id, interval_ms)) {
return MAV_RESULT_ACCEPTED;
}