From 34503de18ea0876905d0bbe28affcaf639c2b21a Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 19 Jan 2015 22:52:54 +0900 Subject: [PATCH] Copter: integrate SerialManager Remove serial0, serial1 baud and protocol parameters Pass serial_manager to GCS, GPS, mount, FrSky_telem objects during init call SerialManager init_console on startup use SerialManager's set_block_writes_all --- ArduCopter/ArduCopter.pde | 2 ++ ArduCopter/Parameters.h | 15 +++----- ArduCopter/Parameters.pde | 39 ++++---------------- ArduCopter/config.h | 18 ---------- ArduCopter/defines.h | 6 ---- ArduCopter/system.pde | 75 +++++++++++++-------------------------- 6 files changed, 38 insertions(+), 117 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index eb44ec4e05..fe9a3e00bb 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -101,6 +101,7 @@ // Application dependencies #include #include // MAVLink GCS definitions +#include // Serial manager library #include // ArduPilot GPS library #include // GPS glitch protection library #include // ArduPilot Mega Flash Memory Library @@ -316,6 +317,7 @@ static float ekfNavVelGainScaler; //////////////////////////////////////////////////////////////////////////////// // GCS selection //////////////////////////////////////////////////////////////////////////////// +static AP_SerialManager serial_manager; static const uint8_t num_gcs = MAVLINK_COMM_NUM_BUFFERS; static GCS_MAVLINK gcs[MAVLINK_COMM_NUM_BUFFERS]; diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 9d79d72e50..50ad45320c 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -115,9 +115,9 @@ public: k_param_poshold_brake_rate, k_param_poshold_brake_angle_max, k_param_pilot_accel_z, - k_param_serial0_baud, - k_param_serial1_baud, - k_param_serial2_baud, + k_param_serial0_baud, // deprecated - remove + k_param_serial1_baud, // deprecated - remove + k_param_serial2_baud, // deprecated - remove k_param_land_repositioning, k_param_sonar, // sonar object k_param_ekfcheck_thresh, @@ -182,7 +182,8 @@ public: k_param_telem_delay, k_param_gcs2, k_param_serial2_baud_old, // deprecated - k_param_serial2_protocol, + k_param_serial2_protocol, // deprecated + k_param_serial_manager, // 119 // // 140: Sensor parameters @@ -325,12 +326,6 @@ public: // AP_Int16 sysid_this_mav; AP_Int16 sysid_my_gcs; - AP_Int16 serial0_baud; - AP_Int16 serial1_baud; -#if MAVLINK_COMM_NUM_BUFFERS > 2 - AP_Int16 serial2_baud; - AP_Int8 serial2_protocol; -#endif AP_Int8 telem_delay; AP_Int16 rtl_altitude; diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 32404285ac..4cb115bbb2 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -53,39 +53,9 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Advanced GSCALAR(sysid_my_gcs, "SYSID_MYGCS", 255), - // @Param: SERIAL0_BAUD - // @DisplayName: USB Console Baud Rate - // @Description: The baud rate used on the USB console. The APM2 can support all baudrates up to 115, and also can support 500. The PX4 can support rates of up to 1500. If you setup a rate you cannot support on APM2 and then can't connect to your board you should load a firmware from a different vehicle type. That will reset all your parameters to defaults. - // @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200,500:500000,921:921600,1500:1500000 - // @User: Standard - GSCALAR(serial0_baud, "SERIAL0_BAUD", SERIAL0_BAUD/1000), - - // @Param: SERIAL1_BAUD - // @DisplayName: Telemetry Baud Rate - // @Description: The baud rate used on the first telemetry port. The APM2 can support all baudrates up to 115, and also can support 500. The PX4 can support rates of up to 1500. If you setup a rate you cannot support on APM2 and then can't connect to your board you should load a firmware from a different vehicle type. That will reset all your parameters to defaults. - // @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200,500:500000,921:921600,1500:1500000 - // @User: Standard - GSCALAR(serial1_baud, "SERIAL1_BAUD", SERIAL1_BAUD/1000), - -#if MAVLINK_COMM_NUM_BUFFERS > 2 - // @Param: SERIAL2_BAUD - // @DisplayName: Telemetry Baud Rate - // @Description: The baud rate used on the second telemetry port. The APM2 can support all baudrates up to 115, and also can support 500. The PX4 can support rates of up to 1500. If you setup a rate you cannot support on APM2 and then can't connect to your board you should load a firmware from a different vehicle type. That will reset all your parameters to defaults. - // @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200,500:500000,921:921600,1500:1500000 - // @User: Standard - - GSCALAR(serial2_baud, "SERIAL2_BAUD", SERIAL2_BAUD/1000), - -#if FRSKY_TELEM_ENABLED == ENABLED - // @Param: SERIAL2_PROTOCOL - // @DisplayName: Serial2 protocol selection - // @Description: Control what protocol telemetry 2 port should be used for - // @Values: 1:GCS Mavlink,2:Frsky D-PORT,3:Frsky S-PORT - // @User: Standard - GSCALAR(serial2_protocol, "SERIAL2_PROTOCOL", SERIAL2_MAVLINK), -#endif // FRSKY_TELEM_ENABLED - -#endif // MAVLINK_COMM_NUM_BUFFERS + // @Group: SERIAL + // @Path: ../libraries/AP_SerialManager/AP_SerialManager.cpp + GOBJECT(serial_manager, "SERIAL", AP_SerialManager), // @Param: TELEM_DELAY // @DisplayName: Telemetry startup delay @@ -1091,6 +1061,9 @@ const AP_Param::ConversionInfo conversion_table[] PROGMEM = { { Parameters::k_param_curr_amp_per_volt, 0, AP_PARAM_FLOAT, "BATT_AMP_PERVOLT" }, { Parameters::k_param_pack_capacity, 0, AP_PARAM_INT32, "BATT_CAPACITY" }, { Parameters::k_param_log_bitmask_old, 0, AP_PARAM_INT16, "LOG_BITMASK" }, + { Parameters::k_param_serial0_baud, 0, AP_PARAM_INT16, "SERIAL0_BAUD" }, + { Parameters::k_param_serial1_baud, 0, AP_PARAM_INT16, "SERIAL1_BAUD" }, + { Parameters::k_param_serial2_baud, 0, AP_PARAM_INT16, "SERIAL2_BAUD" }, }; static void load_parameters(void) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index bb9be3a830..b24d572a9d 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -58,10 +58,6 @@ #define CONFIG_BARO HAL_BARO_DEFAULT #define CONFIG_COMPASS HAL_COMPASS_DEFAULT -#ifdef HAL_SERIAL0_BAUD_DEFAULT -# define SERIAL0_BAUD HAL_SERIAL0_BAUD_DEFAULT -#endif - ////////////////////////////////////////////////////////////////////////////// // HIL_MODE OPTIONAL @@ -259,20 +255,6 @@ #endif -////////////////////////////////////////////////////////////////////////////// -// Serial port speeds. -// -#ifndef SERIAL0_BAUD - # define SERIAL0_BAUD 115200 -#endif -#ifndef SERIAL1_BAUD - # define SERIAL1_BAUD 57600 -#endif -#ifndef SERIAL2_BAUD - # define SERIAL2_BAUD 57600 -#endif - - ////////////////////////////////////////////////////////////////////////////// // Battery monitoring // diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 5899663ab5..972d2eba7c 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -392,10 +392,4 @@ enum FlipState { #define MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE (1<<11) #define MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE (1<<12) -enum Serial2Protocol { - SERIAL2_MAVLINK = 1, - SERIAL2_FRSKY_DPORT = 2, - SERIAL2_FRSKY_SPORT = 3 // not supported yet -}; - #endif // _DEFINES_H diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 6ec6d24b8f..cf70bc985b 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -82,32 +82,8 @@ static void init_ardupilot() delay(1000); } - // Console serial port - // - // The console port buffers are defined to be sufficiently large to support - // the MAVLink protocol efficiently - // -#if HIL_MODE != HIL_MODE_DISABLED - // we need more memory for HIL, as we get a much higher packet rate - hal.uartA->begin(map_baudrate(g.serial0_baud), 256, 256); -#else - // use a bit less for non-HIL operation - hal.uartA->begin(map_baudrate(g.serial0_baud), 512, 128); -#endif - - // GPS serial port. - // -#if GPS_PROTOCOL != GPS_PROTOCOL_IMU - // standard gps running. Note that we need a 256 byte buffer for some - // GPS types (eg. UBLOX) - hal.uartB->begin(38400, 256, 16); -#endif - -#if GPS2_ENABLE - if (hal.uartE != NULL) { - hal.uartE->begin(38400, 256, 16); - } -#endif + // initialise serial port + serial_manager.init_console(); cliSerial->printf_P(PSTR("\n\nInit " FIRMWARE_STRING "\n\nFree RAM: %u\n"), @@ -131,6 +107,9 @@ static void init_ardupilot() BoardConfig.init(); + // initialise serial port + serial_manager.init(); + // init EPM cargo gripper #if EPM_ENABLED == ENABLED epm.init(); @@ -147,8 +126,8 @@ static void init_ardupilot() barometer.init(); - // init the GCS - gcs[0].init(hal.uartA); + // init the GCS connected to the console + gcs[0].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_Console); // Register the mavlink service callback. This will run // anytime there are more than 5ms remaining in a call to @@ -164,16 +143,17 @@ 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), 128, 128); + gcs[1].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink1); #endif #if MAVLINK_COMM_NUM_BUFFERS > 2 - if (g.serial2_protocol == SERIAL2_FRSKY_DPORT || - g.serial2_protocol == SERIAL2_FRSKY_SPORT) { - frsky_telemetry.init(hal.uartD, (AP_Frsky_Telem::FrSkyProtocol)g.serial2_protocol.get()); - } else { - gcs[2].setup_uart(hal.uartD, map_baudrate(g.serial2_baud), 128, 128); - } + // setup serial port for telem2 + gcs[2].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink2); +#endif + +#if FRSKY_TELEM_ENABLED == ENABLED + // setup frsky + frsky_telemetry.init(serial_manager); #endif // identify ourselves correctly with the ground station @@ -211,7 +191,7 @@ static void init_ardupilot() #endif // CONFIG_ADC // Do GPS init - gps.init(&DataFlash); + gps.init(&DataFlash, serial_manager); if(g.compass_enabled) init_compass(); @@ -232,7 +212,7 @@ static void init_ardupilot() inertial_nav.init(); // initialise camera mount - camera_mount.init(); + camera_mount.init(serial_manager); #ifdef USERHOOK_INIT USERHOOK_INIT @@ -241,11 +221,11 @@ static void init_ardupilot() #if CLI_ENABLED == ENABLED const prog_char_t *msg = PSTR("\nPress ENTER 3 times to start interactive setup\n"); cliSerial->println_P(msg); - if (gcs[1].initialised) { - hal.uartC->println_P(msg); + if (gcs[1].initialised && (gcs[1].get_uart() != NULL)) { + gcs[1].get_uart()->println_P(msg); } - if (num_gcs > 2 && gcs[2].initialised) { - hal.uartD->println_P(msg); + if (num_gcs > 2 && gcs[2].initialised && (gcs[2].get_uart() != NULL)) { + gcs[2].get_uart()->println_P(msg); } #endif // CLI_ENABLED @@ -292,12 +272,7 @@ static void init_ardupilot() // we don't want writes to the serial port to cause us to pause // mid-flight, so set the serial ports non-blocking once we are // ready to fly - hal.uartA->set_blocking_writes(false); - hal.uartB->set_blocking_writes(false); - hal.uartC->set_blocking_writes(false); - if (hal.uartD != NULL) { - hal.uartD->set_blocking_writes(false); - } + serial_manager.set_blocking_writes_all(false); cliSerial->print_P(PSTR("\nReady to FLY ")); @@ -428,10 +403,10 @@ static void check_usb_mux(void) // between USB and a TTL serial connection. When on USB we use // SERIAL0_BAUD, but when connected as a TTL serial port we run it // at SERIAL1_BAUD. - if (ap.usb_connected) { - hal.uartA->begin(map_baudrate(g.serial0_baud)); + if (usb_connected) { + serial_manager.set_console_baud(AP_SerialManager::SerialProtocol_Console); } else { - hal.uartA->begin(map_baudrate(g.serial1_baud)); + serial_manager.set_console_baud(AP_SerialManager::SerialProtocol_MAVLink1); } #endif }