From c7fb252fbb5059a49424bbf92e7530d5dfa70ffd Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 28 Jan 2015 14:27:03 +0900 Subject: [PATCH] Tracker: integrate SerialManager --- AntennaTracker/AntennaTracker.pde | 2 ++ AntennaTracker/Parameters.h | 19 +++++++++++-------- AntennaTracker/Parameters.pde | 27 ++++----------------------- AntennaTracker/config.h | 28 ---------------------------- AntennaTracker/system.pde | 28 ++++++++++++---------------- 5 files changed, 29 insertions(+), 75 deletions(-) diff --git a/AntennaTracker/AntennaTracker.pde b/AntennaTracker/AntennaTracker.pde index 625fa331a7..e934053877 100644 --- a/AntennaTracker/AntennaTracker.pde +++ b/AntennaTracker/AntennaTracker.pde @@ -46,6 +46,7 @@ #include #include // MAVLink GCS definitions +#include // Serial manager library #include // ArduPilot Mega Declination Helper Library #include #include @@ -159,6 +160,7 @@ static RC_Channel channel_pitch(CH_PITCH); //////////////////////////////////////////////////////////////////////////////// // 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/AntennaTracker/Parameters.h b/AntennaTracker/Parameters.h index 9b749433be..42002cb3ff 100644 --- a/AntennaTracker/Parameters.h +++ b/AntennaTracker/Parameters.h @@ -55,8 +55,8 @@ public: k_param_gcs1, // stream rates for uartC k_param_sysid_this_mav, k_param_sysid_my_gcs, - k_param_serial0_baud, - k_param_serial1_baud, + k_param_serial0_baud, // deprecated + k_param_serial1_baud, // deprecated k_param_imu, k_param_compass_enabled, k_param_compass, @@ -68,7 +68,7 @@ public: k_param_pidPitch2Srv, k_param_pidYaw2Srv, k_param_gcs2, // stream rates for uartD - k_param_serial2_baud, + k_param_serial2_baud, // deprecated k_param_yaw_slew_time, k_param_pitch_slew_time, @@ -91,6 +91,14 @@ public: k_param_yaw_range, k_param_pitch_range, // 136 + // + // 150: Telemetry control + // + k_param_serial_manager, // serial manager library + + // + // 200 : Radio settings + // k_param_channel_yaw = 200, k_param_channel_pitch, @@ -109,11 +117,6 @@ public: // AP_Int16 sysid_this_mav; AP_Int16 sysid_my_gcs; - AP_Int8 serial0_baud; - AP_Int8 serial1_baud; -#if MAVLINK_COMM_NUM_BUFFERS > 2 - AP_Int8 serial2_baud; -#endif AP_Int8 compass_enabled; diff --git a/AntennaTracker/Parameters.pde b/AntennaTracker/Parameters.pde index 3b3b78b38d..d617cf1d70 100644 --- a/AntennaTracker/Parameters.pde +++ b/AntennaTracker/Parameters.pde @@ -29,29 +29,6 @@ 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 - // @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200 - // @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 - // @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200 - // @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 - // @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200 - // @User: Standard - GSCALAR(serial2_baud, "SERIAL2_BAUD", SERIAL2_BAUD/1000), -#endif - // @Param: MAG_ENABLE // @DisplayName: Enable Compass // @Description: Setting this to Enabled(1) will enable the compass. Setting this to Disabled(0) will disable the compass. Note that this is separate from COMPASS_USE. This will enable the low level senor, and will enable logging of magnetometer data. To use the compass for navigation you must also set COMPASS_USE to 1. @@ -262,6 +239,10 @@ const AP_Param::Info var_info[] PROGMEM = { // @Path: ../libraries/RC_Channel/RC_Channel.cpp GOBJECT(channel_pitch, "RC2_", RC_Channel), + // @Group: SERIAL + // @Path: ../libraries/AP_SerialManager/AP_SerialManager.cpp + GOBJECT(serial_manager, "SERIAL", AP_SerialManager), + GGROUP(pidPitch2Srv, "PITCH2SRV_", PID), GGROUP(pidYaw2Srv, "YAW2SRV_", PID), diff --git a/AntennaTracker/config.h b/AntennaTracker/config.h index d1f49e4eb9..4960dd6136 100644 --- a/AntennaTracker/config.h +++ b/AntennaTracker/config.h @@ -20,39 +20,11 @@ #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 - #ifndef MAV_SYSTEM_ID // use 2 for antenna tracker by default # define MAV_SYSTEM_ID 2 #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 - -#ifndef SERIAL_BUFSIZE - # define SERIAL_BUFSIZE 512 -#endif - -#ifndef SERIAL1_BUFSIZE - # define SERIAL1_BUFSIZE 256 -#endif - -#ifndef SERIAL2_BUFSIZE - # define SERIAL2_BUFSIZE 256 -#endif ////////////////////////////////////////////////////////////////////////////// // RC Channel definitions diff --git a/AntennaTracker/system.pde b/AntennaTracker/system.pde index 29aea5a448..b0ad65e5eb 100644 --- a/AntennaTracker/system.pde +++ b/AntennaTracker/system.pde @@ -5,10 +5,8 @@ static const StorageAccess wp_storage(StorageManager::StorageMission); static void init_tracker() { - hal.uartA->begin(SERIAL0_BAUD, 128, SERIAL_BUFSIZE); - - // gps port - hal.uartB->begin(38400, 256, 16); + // initialise console serial port + serial_manager.init_console(); cliSerial->printf_P(PSTR("\n\nInit " THISFIRMWARE "\n\nFree RAM: %u\n"), @@ -19,14 +17,14 @@ static void init_tracker() BoardConfig.init(); - // reset the uartA baud rate after parameter load - hal.uartA->begin(map_baudrate(g.serial0_baud)); + // initialise serial ports + serial_manager.init(); // init baro before we start the GCS, so that the CLI baro test works barometer.init(); // init the GCS - gcs[0].init(hal.uartA); + gcs[0].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_Console); // set up snooping on other mavlink destinations gcs[0].set_snoop(mavlink_snoop); @@ -40,9 +38,8 @@ static void init_tracker() usb_connected = true; check_usb_mux(); - // we have a 2nd serial port for telemetry - hal.uartC->begin(map_baudrate(g.serial1_baud), 128, SERIAL1_BUFSIZE); - gcs[1].setup_uart(hal.uartC, map_baudrate(g.serial1_baud), 128, SERIAL1_BUFSIZE); + // setup serial port for telem1 + gcs[1].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink1); mavlink_system.sysid = g.sysid_this_mav; @@ -56,7 +53,7 @@ static void init_tracker() } // GPS Initialization - gps.init(NULL); + gps.init(NULL, serial_manager); ahrs.init(); ahrs.set_fly_forward(false); @@ -66,9 +63,8 @@ static void init_tracker() init_barometer(); - hal.uartA->set_blocking_writes(false); - hal.uartB->set_blocking_writes(false); - hal.uartC->set_blocking_writes(false); + // set serial ports non-blocking + serial_manager.set_blocking_writes_all(false); // initialise servos init_servos(); @@ -233,9 +229,9 @@ static void check_usb_mux(void) // SERIAL0_BAUD, but when connected as a TTL serial port we run it // at SERIAL1_BAUD. if (usb_connected) { - hal.uartA->begin(SERIAL0_BAUD); + 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 }