diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 557c9db2be292560a42c512f9ea620032048fbaf..1b14c22d3530d1a62131cb39e7607f3acb9510cc 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -150,41 +150,40 @@ static void init_ardupilot() // load parameters from EEPROM load_parameters(); - // init the GCS - gcs0.init(&Serial); -#if USB_MUX_PIN > 0 - if (!usb_connected) { - // we are not connected via USB, re-init UART0 with right - // baud rate - Serial.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD)); +#ifdef CELLULAR_MODEM_INIT - #ifdef CELLULAR_MODEM_INIT // on APM2 board, cellular modem on Serial0, same as console USB port and Xbee. - Cellular_Modem modem(&Serial); + #ifdef CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 // we're on an APM2 board // Before starting GCS on Serial, initialize the modem - SendDebug("\nModem intialization: "); + SendDebug("\nModem intialization on Serial0: "); + Cellular_Modem modem(&Serial); + #else + SendDebug("\nModem intialization on Serial3: "); + Cellular_Modem modem(&Serial3); + #endif // APM1 or APM2 + if (modem.init_modem()) { SendDebug("success.\n"); } - else + else { SendDebug("failed.\n"); - #endif // CELLULAR_MODEM_INIT + } + +#endif // CELLULAR_MODEM_INIT + + // init the GCS + gcs0.init(&Serial); + +#if USB_MUX_PIN > 0 + if (!usb_connected) { + // we are not connected via USB, re-init UART0 with right + // baud rate + Serial.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 256); } #else // we have a 2nd serial port for telemetry Serial3.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 256); - #ifdef CELLULAR_MODEM_INIT // on APM1 board, cellular modem on Serial3, same as Xbee - Cellular_Modem modem_on_s3(&Serial3); - // Before starting GCS on Serial3, initialize the modem - SendDebug("\nModem intialization: "); - if (modem_on_s3.init_modem()) { - SendDebug("success.\n"); - } - else - SendDebug("failed.\n"); - #endif // CELLULAR_MODEM_INIT - gcs3.init(&Serial3); #endif diff --git a/libraries/Cellular_Modem/Cellular_Modem.cpp b/libraries/Cellular_Modem/Cellular_Modem.cpp index 1f8eaa0c5204e15a053dcb66add59e3befa5af2a..5374a8f2bcd4ea19a42cb7e025833af571708b29 100644 --- a/libraries/Cellular_Modem/Cellular_Modem.cpp +++ b/libraries/Cellular_Modem/Cellular_Modem.cpp @@ -113,7 +113,7 @@ bool Cellular_Modem::init_modem() { } p->println(); // Send the newline to "enter" the command - Serial.printf("\n--MODEM: [%s]\n",init_script[i]); // DEBUG + //Serial.printf("\n--MODEM: [%s]\n",init_script[i]); // DEBUG // loop until response -- need breaker to avoid inf loop start = millis(); // for timeout counting @@ -140,7 +140,7 @@ bool Cellular_Modem::init_modem() { // experimental if (this->parse_status(resp_buffer)) { // look for "OK" or "0" - Serial.printf("Success: %s",resp_buffer); // DEBUG + //Serial.printf("Success: %s",resp_buffer); // DEBUG break; // send next line } } while (p->available() && (millis() - start) < RESP_TIMEOUT); @@ -149,7 +149,7 @@ bool Cellular_Modem::init_modem() { delay(LINE_DELAY); } // we're done, without an error - Serial.printf("--MODEM Init: %lu msec", millis() - init_time); // DEBUG + //Serial.printf("--MODEM Init: %lu msec", millis() - init_time); // DEBUG return true; // successful init //send_test(); } diff --git a/libraries/Cellular_Modem/Cellular_Modem.h b/libraries/Cellular_Modem/Cellular_Modem.h index 89696de39491338cebfcd894a3fffe8616678430..52abc522b70878dbe112bd12de1f29c2f456a24d 100644 --- a/libraries/Cellular_Modem/Cellular_Modem.h +++ b/libraries/Cellular_Modem/Cellular_Modem.h @@ -27,9 +27,9 @@ // * (# of lines in script). // #define CHAR_DELAY 10 // number of milliseconds to wait between sending each character - most modems needs to be "paced" -#define RESP_DELAY 500 // number of milliseconds to wait before checking for a response -#define LINE_DELAY 500 // number of milliseconds to wait after each line is sent -#define RESP_TIMEOUT 40000 // How many milliseconds to wait for a response before giving up with an error - 30 seconds by default +#define RESP_DELAY 400 // number of milliseconds to wait before checking for a response +#define LINE_DELAY 400 // number of milliseconds to wait after each line is sent +#define RESP_TIMEOUT 20000 // How many milliseconds to wait for a response before giving up with an error - 20 seconds by default #define RESP_BUFFER 512 // size in bytes of response buffer // @@ -71,10 +71,10 @@ // TODO: UI for user to change these values in runtime or by MAVLINK PARAM // rather than having to edit source code -#define VAL_APN "epc.tmobile.com" // value for the APN +#define VAL_APN "epc.tmobile.com" // value for the APN - try "internet" also #define VAL_USER "" // value for the user name #define VAL_PW "" // value for the password -#define VAL_IP "uav.antonopoulos.com" // value for the IP address +#define VAL_IP "uav.willnotwork.example.com" // value for the IP address #define VAL_PROTO "udp" // value for the protocol #define VAL_PORT "14550"