aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

GCS_MAVLink: moved RADIO_STATUS handling into common code

Browse Source
This commit is contained in:
Andrew Tridgell 2014-03-19 08:44:58 +11:00 committed by Randy Mackay
parent 144217ac74
commit 79bb14b8c1
2 changed files with 35 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/GCS_MAVLink/GCS.h
View File

@ -179,6 +179,9 @@ public:
uint32_t last_heartbeat_time; // milliseconds
// last time we got a non-zero RSSI from RADIO_STATUS
static uint32_t last_radio_status_remrssi_ms;
// common send functions
void send_meminfo(void);
void send_power_status(void);
@ -297,6 +300,7 @@ private:
void handle_param_request_list(mavlink_message_t *msg);
void handle_param_request_read(mavlink_message_t *msg);
void handle_param_set(mavlink_message_t *msg, DataFlash_Class &DataFlash);
void handle_radio_status(mavlink_message_t *msg);
// return true if this channel has hardware flow control
bool have_flow_control(void);

31
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -22,6 +22,8 @@
extern const AP_HAL::HAL& hal;
uint32_t GCS_MAVLINK::last_radio_status_remrssi_ms;
GCS_MAVLINK::GCS_MAVLINK() :
waypoint_receive_timeout(1000)
{
@ -595,3 +597,32 @@ GCS_MAVLINK::send_text_P(gcs_severity severity, const prog_char_t *str)
send_text(severity, (const char *)m.text);
}
void GCS_MAVLINK::handle_radio_status(mavlink_message_t *msg)
{
mavlink_radio_t packet;
mavlink_msg_radio_decode(msg, &packet);
// record if the GCS has been receiving radio messages from
// the aircraft
if (packet.remrssi != 0) {
last_radio_status_remrssi_ms = hal.scheduler->millis();
}
// use the state of the transmit buffer in the radio to
// control the stream rate, giving us adaptive software
// flow control
if (packet.txbuf < 20 && stream_slowdown < 100) {
// we are very low on space - slow down a lot
stream_slowdown += 3;
} else if (packet.txbuf < 50 && stream_slowdown < 100) {
// we are a bit low on space, slow down slightly
stream_slowdown += 1;
} else if (packet.txbuf > 95 && stream_slowdown > 10) {
// the buffer has plenty of space, speed up a lot
stream_slowdown -= 2;
} else if (packet.txbuf > 90 && stream_slowdown != 0) {
// the buffer has enough space, speed up a bit
stream_slowdown--;
}
}