Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: add set_message_interval functions

Browse Source
This commit is contained in:
Tatsuya Yamaguchi 2019-09-11 10:26:15 +09:00 committed by Andrew Tridgell
parent 9cb36fc1de
commit 48307e2268
2 changed files with 35 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/GCS_MAVLink/GCS.h
View File

@ -273,6 +273,8 @@ public:
virtual uint64_t capabilities() const;
uint8_t get_stream_slowdown_ms() const { return stream_slowdown_ms; }
MAV_RESULT set_message_interval(uint32_t msg_id, int32_t interval_us);
protected:
virtual bool in_hil_mode() const { return false; }
@ -786,6 +788,9 @@ public:
virtual bool simple_input_active() const { return false; }
virtual bool supersimple_input_active() const { return false; }
MAV_RESULT set_message_interval(uint8_t port_num, uint32_t msg_id, int32_t interval_us);
uint8_t get_channel_from_port_number(uint8_t port_num);
protected:
virtual GCS_MAVLINK *new_gcs_mavlink_backend(GCS_MAVLINK_Parameters &params,

32
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -2231,9 +2231,11 @@ void GCS_MAVLINK::send_heartbeat() const
MAV_RESULT GCS_MAVLINK::handle_command_set_message_interval(const mavlink_command_long_t &packet)
{
const uint32_t msg_id = (uint32_t)packet.param1;
const int32_t interval_us = (int32_t)packet.param2;
return set_message_interval((uint32_t)packet.param1, (int32_t)packet.param2);
}
MAV_RESULT GCS_MAVLINK::set_message_interval(uint32_t msg_id, int32_t interval_us)
{
uint16_t interval_ms;
if (interval_us == 0) {
// zero is "reset to default rate"
@ -2258,6 +2260,32 @@ MAV_RESULT GCS_MAVLINK::handle_command_set_message_interval(const mavlink_comman
return MAV_RESULT_FAILED;
}
/*
this function is reserved for use by scripting
*/
MAV_RESULT GCS::set_message_interval(uint8_t port_num, uint32_t msg_id, int32_t interval_us)
{
uint8_t channel = get_channel_from_port_number(port_num);
if (channel < MAVLINK_COMM_NUM_BUFFERS && chan(channel) != nullptr)
{
chan(channel)->set_message_interval(msg_id, interval_us);
}
return MAV_RESULT_FAILED;
}
uint8_t GCS::get_channel_from_port_number(uint8_t port_num)
{
for (uint8_t i=0; i<num_gcs(); i++) {
if (chan(i)->get_uart() == AP::serialmanager().get_serial_by_id(port_num)) {
return i;
}
}
return UINT8_MAX;
}
MAV_RESULT GCS_MAVLINK::handle_command_request_message(const mavlink_command_long_t &packet)
{
const uint32_t mavlink_id = (uint32_t)packet.param1;