Browse Source

GCS_MAVLink: added have_flow_control() method

can be used to change speed of handling some protocol methods, as we
know communication will be reliable
master
Andrew Tridgell 11 years ago committed by Randy Mackay
parent
commit
0b811ba6a9
  1. 4
      libraries/GCS_MAVLink/GCS.h
  2. 19
      libraries/GCS_MAVLink/GCS_Common.cpp
  3. 7
      libraries/GCS_MAVLink/GCS_Logs.cpp

4
libraries/GCS_MAVLink/GCS.h

@ -287,10 +287,14 @@ private: @@ -287,10 +287,14 @@ private:
void handle_mission_request_list(AP_Mission &mission, mavlink_message_t *msg);
void handle_mission_request(AP_Mission &mission, mavlink_message_t *msg,
AP_Mission::Mission_Command &cmd);
void handle_mission_set_current(AP_Mission &mission, mavlink_message_t *msg);
void handle_mission_count(AP_Mission &mission, mavlink_message_t *msg);
void handle_mission_clear_all(AP_Mission &mission, mavlink_message_t *msg);
void handle_mission_write_partial_list(AP_Mission &mission, mavlink_message_t *msg);
// return true if this channel has hardware flow control
bool have_flow_control(void);
};
#endif // __GCS_H

19
libraries/GCS_MAVLink/GCS_Common.cpp

@ -365,3 +365,22 @@ void GCS_MAVLINK::handle_mission_write_partial_list(AP_Mission &mission, mavlink @@ -365,3 +365,22 @@ void GCS_MAVLINK::handle_mission_write_partial_list(AP_Mission &mission, mavlink
waypoint_request_i = packet.start_index;
waypoint_request_last= packet.end_index;
}
/*
return true if a channel has flow control
*/
bool GCS_MAVLINK::have_flow_control(void)
{
switch (chan) {
case MAVLINK_COMM_0:
// assume USB has flow control
return hal.gpio->usb_connected() || hal.uartA->get_flow_control() != AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE;
case MAVLINK_COMM_1:
return hal.uartC->get_flow_control() != AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE;
case MAVLINK_COMM_2:
return hal.uartD != NULL && hal.uartD->get_flow_control() != AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE;
}
return false;
}

7
libraries/GCS_MAVLink/GCS_Logs.cpp

@ -141,12 +141,7 @@ void GCS_MAVLINK::handle_log_send(DataFlash_Class &dataflash) @@ -141,12 +141,7 @@ void GCS_MAVLINK::handle_log_send(DataFlash_Class &dataflash)
if (chan == MAVLINK_COMM_0 && hal.gpio->usb_connected()) {
// when on USB we can send a lot more data
num_sends = 40;
} else if (chan == MAVLINK_COMM_1 &&
hal.uartC->get_flow_control() != AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE) {
num_sends = 10;
} else if (chan == MAVLINK_COMM_2 &&
hal.uartD != NULL &&
hal.uartD->get_flow_control() != AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE) {
} else if (have_flow_control()) {
num_sends = 10;
}

Loading…
Cancel
Save