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

Copter: updates for new GCS_MAVLink API

This commit is contained in:
Andrew Tridgell 2014-05-21 12:43:46 +10:00
parent cdcaad3079
commit a55c511f63
2 changed files with 11 additions and 88 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
ArduCopter/GCS_Mavlink.pde
View File

@ -9,9 +9,6 @@ static bool do_guided(const AP_Mission::Mission_Command& cmd);
// use this to prevent recursion during sensor init
static bool in_mavlink_delay;
// true when we have received at least 1 MAVLink packet
static bool mavlink_active;
// true if we are out of time in our event timeslice
static bool gcs_out_of_time;
@ -867,64 +864,6 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] PROGMEM = {
AP_GROUPEND
};
void
GCS_MAVLINK::update(void)
{
// receive new packets
mavlink_message_t msg;
mavlink_status_t status;
status.packet_rx_drop_count = 0;
// process received bytes
uint16_t nbytes = comm_get_available(chan);
for (uint16_t i=0; i<nbytes; i++)
{
uint8_t c = comm_receive_ch(chan);
#if CLI_ENABLED == ENABLED
/* allow CLI to be started by hitting enter 3 times, if no
* heartbeat packets have been received */
if (mavlink_active == 0 && (millis() - _cli_timeout) < 20000 &&
!motors.armed() && comm_is_idle(chan)) {
if (c == '\n' || c == '\r') {
crlf_count++;
} else {
crlf_count = 0;
}
if (crlf_count == 3) {
run_cli(_port);
}
}
#endif
// Try to get a new message
if (mavlink_parse_char(chan, c, &msg, &status)) {
// we exclude radio packets to make it possible to use the
// CLI over the radio
if (msg.msgid != MAVLINK_MSG_ID_RADIO && msg.msgid != MAVLINK_MSG_ID_RADIO_STATUS) {
mavlink_active = true;
}
handleMessage(&msg);
}
}
// handle receiving commands from GCS
if (waypoint_receiving) {
uint32_t tnow = millis();
// request another command from the GCS if at least 500ms has passed
if (waypoint_request_i <= waypoint_request_last &&
tnow > waypoint_timelast_request + 500 + (stream_slowdown*20)) {
waypoint_timelast_request = tnow;
send_message(MSG_NEXT_WAYPOINT);
}
// stop waypoint receiving if timeout
if ((tnow - waypoint_timelast_receive) > waypoint_receive_timeout) {
waypoint_receiving = false;
}
}
}
// see if we should send a stream now. Called at 50Hz
bool GCS_MAVLINK::stream_trigger(enum streams stream_num)
@ -1616,7 +1555,11 @@ static void gcs_check_input(void)
{
for (uint8_t i=0; i<num_gcs; i++) {
if (gcs[i].initialised) {
gcs[i].update();
#if CLI_ENABLED == ENABLED
gcs[i].update(run_cli);
#else
gcs[i].update(NULL);
#endif
}
}
}

32
ArduCopter/system.pde
View File

@ -89,10 +89,10 @@ static void init_ardupilot()
//
#if HIL_MODE != HIL_MODE_DISABLED
// we need more memory for HIL, as we get a much higher packet rate
hal.uartA->begin(SERIAL0_BAUD, 256, 256);
hal.uartA->begin(map_baudrate(g.serial0_baud), 256, 256);
#else
// use a bit less for non-HIL operation
hal.uartA->begin(SERIAL0_BAUD, 512, 128);
hal.uartA->begin(map_baudrate(g.serial0_baud), 512, 128);
#endif
// GPS serial port.
@ -186,10 +186,10 @@ static void init_ardupilot()
// we have a 2nd serial port for telemetry on all boards except
// APM2. We actually do have one on APM2 but it isn't necessary as
// a MUX is used
gcs[1].setup_uart(hal.uartC, map_baudrate(g.serial1_baud, SERIAL1_BAUD), 128, 128);
gcs[1].setup_uart(hal.uartC, map_baudrate(g.serial1_baud), 128, 128);
#endif
#if MAVLINK_COMM_NUM_BUFFERS > 2
gcs[2].setup_uart(hal.uartD, map_baudrate(g.serial2_baud, SERIAL2_BAUD), 128, 128);
gcs[2].setup_uart(hal.uartD, map_baudrate(g.serial2_baud), 128, 128);
#endif
// identify ourselves correctly with the ground station
@ -364,26 +364,6 @@ static void update_auto_armed()
}
}
/*
* map from a 8 bit EEPROM baud rate to a real baud rate
*/
static uint32_t map_baudrate(int8_t rate, uint32_t default_baud)
{
switch (rate) {
case 1: return 1200;
case 2: return 2400;
case 4: return 4800;
case 9: return 9600;
case 19: return 19200;
case 38: return 38400;
case 57: return 57600;
case 111: return 111100;
case 115: return 115200;
}
//cliSerial->println_P(PSTR("Invalid baudrate"));
return default_baud;
}
static void check_usb_mux(void)
{
bool usb_check = hal.gpio->usb_connected();
@ -400,9 +380,9 @@ static void check_usb_mux(void)
// SERIAL0_BAUD, but when connected as a TTL serial port we run it
// at SERIAL1_BAUD.
if (ap.usb_connected) {
hal.uartA->begin(SERIAL0_BAUD);
hal.uartA->begin(map_baudrate(g.serial0_baud));
} else {
hal.uartA->begin(map_baudrate(g.serial1_baud, SERIAL1_BAUD));
hal.uartA->begin(map_baudrate(g.serial1_baud));
}
#endif
}