From 9f7f1e62df7d9db9756b5da0258a746879f64c7c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 28 Jan 2015 13:56:36 +0900 Subject: [PATCH] Rover: integrate SerialManager --- APMrover2/APMrover2.pde | 3 +- APMrover2/Parameters.h | 15 +++----- APMrover2/Parameters.pde | 41 ++++---------------- APMrover2/config.h | 16 -------- APMrover2/defines.h | 6 --- APMrover2/system.pde | 82 +++++++++++++++------------------------- 6 files changed, 45 insertions(+), 118 deletions(-) diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde index 11d7e49ec9..ec08f715b8 100644 --- a/APMrover2/APMrover2.pde +++ b/APMrover2/APMrover2.pde @@ -88,6 +88,7 @@ #include // Camera/Antenna mount #include // Camera triggering #include // MAVLink GCS definitions +#include // Serial manager library #include // needed for AHRS build #include // needed for AHRS build #include @@ -256,7 +257,7 @@ SITL sitl; //////////////////////////////////////////////////////////////////////////////// // 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/APMrover2/Parameters.h b/APMrover2/Parameters.h index ad336d89ed..daaafc2c36 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -51,9 +51,9 @@ public: // misc2 k_param_log_bitmask = 40, k_param_gps, - k_param_serial0_baud, - k_param_serial1_baud, - k_param_serial2_baud, + k_param_serial0_baud, // deprecated, can be deleted + k_param_serial1_baud, // deprecated, can be deleted + k_param_serial2_baud, // deprecated, can be deleted // 110: Telemetry control @@ -68,7 +68,8 @@ public: k_param_skip_gyro_cal, k_param_gcs2, // stream rates for uartD k_param_serial2_baud_old, - k_param_serial2_protocol, + k_param_serial2_protocol, // deprecated, can be deleted + k_param_serial_manager, // serial manager library // // 130: Sensor parameters @@ -210,12 +211,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_Int8 skip_gyro_cal; diff --git a/APMrover2/Parameters.pde b/APMrover2/Parameters.pde index 93d76d3379..fc0674d19f 100644 --- a/APMrover2/Parameters.pde +++ b/APMrover2/Parameters.pde @@ -56,40 +56,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. 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 - - // @Param: TELEM_DELAY // @DisplayName: Telemetry startup delay // @Description: The amount of time (in seconds) to delay radio telemetry to prevent an Xbee bricking on power up @@ -489,6 +455,10 @@ const AP_Param::Info var_info[] PROGMEM = { GOBJECTN(gcs[2], gcs2, "SR2_", GCS_MAVLINK), #endif + // @Group: SERIAL + // @Path: ../libraries/AP_SerialManager/AP_SerialManager.cpp + GOBJECT(serial_manager, "SERIAL", AP_SerialManager), + // @Group: NAVL1_ // @Path: ../libraries/AP_L1_Control/AP_L1_Control.cpp GOBJECT(L1_controller, "NAVL1_", AP_L1_Control), @@ -569,6 +539,9 @@ const AP_Param::ConversionInfo conversion_table[] PROGMEM = { { Parameters::k_param_volt_div_ratio, 0, AP_PARAM_FLOAT, "BATT_VOLT_MULT" }, { 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_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/APMrover2/config.h b/APMrover2/config.h index 11d1581f1e..27c9830d1b 100644 --- a/APMrover2/config.h +++ b/APMrover2/config.h @@ -54,10 +54,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 @@ -106,18 +102,6 @@ # define MAV_SYSTEM_ID 1 #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 ////////////////////////////////////////////////////////////////////////////// // FrSky telemetry support diff --git a/APMrover2/defines.h b/APMrover2/defines.h index b49e21938c..82111f5ac0 100644 --- a/APMrover2/defines.h +++ b/APMrover2/defines.h @@ -126,10 +126,4 @@ enum mode { // mark a function as not to be inlined #define NOINLINE __attribute__((noinline)) -enum Serial2Protocol { - SERIAL2_MAVLINK = 1, - SERIAL2_FRSKY_DPORT = 2, - SERIAL2_FRSKY_SPORT = 3 // not supported yet -}; - #endif // _DEFINES_H diff --git a/APMrover2/system.pde b/APMrover2/system.pde index eec834c728..81ac2785ef 100644 --- a/APMrover2/system.pde +++ b/APMrover2/system.pde @@ -71,34 +71,8 @@ static void run_cli(AP_HAL::UARTDriver *port) static void init_ardupilot() { - // Console serial port - // - // The console port buffers are defined to be sufficiently large to support - // the console's use as a logging device, optionally as the GPS port when - // GPS_PROTOCOL_IMU is selected, and as the telemetry port. - // - // XXX This could be optimised to reduce the buffer sizes in the cases - // where they are not otherwise required. - // - hal.uartA->begin(SERIAL0_BAUD, 128, 128); - - // GPS serial port. - // - // XXX currently the EM406 (SiRF receiver) is nominally configured - // at 57600, however it's not been supported to date. We should - // probably standardise on 38400. - // - // XXX the 128 byte receive buffer may be too small for NMEA, depending - // on the message set configured. - // - // standard gps running - hal.uartB->begin(38400, 256, 16); - -#if GPS2_ENABLE - if (hal.uartE != NULL) { - hal.uartE->begin(38400, 256, 16); - } -#endif + // initialise console serial port + serial_manager.init_console(); cliSerial->printf_P(PSTR("\n\nInit " FIRMWARE_STRING "\n\nFree RAM: %u\n"), @@ -112,13 +86,13 @@ static void init_ardupilot() BoardConfig.init(); + // initialise serial ports + serial_manager.init(); + ServoRelayEvents.set_channel_mask(0xFFF0); set_control_channels(); - // after parameter load setup correct baud rate on uartA - hal.uartA->begin(map_baudrate(g.serial0_baud)); - // keep a record of how many resets have happened. This can be // used to detect in-flight resets g.num_resets.set_and_save(g.num_resets+1); @@ -127,23 +101,24 @@ static void init_ardupilot() barometer.init(); // init the GCS - gcs[0].init(hal.uartA); + gcs[0].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_Console); // we start by assuming USB connected, as we initialed the serial // port with SERIAL0_BAUD. check_usb_mux() fixes this if need be. usb_connected = true; check_usb_mux(); - // we have a 2nd serial port for telemetry - gcs[1].setup_uart(hal.uartC, map_baudrate(g.serial1_baud), 128, 128); + // setup serial port for telem1 + gcs[1].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink1); #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 + + // setup frsky telemetry +#if FRSKY_TELEM_ENABLED == ENABLED + frsky_telemetry.init(serial_manager); #endif mavlink_system.sysid = g.sysid_this_mav; @@ -187,7 +162,7 @@ static void init_ardupilot() init_barometer(); // Do GPS init - gps.init(&DataFlash); + gps.init(&DataFlash, serial_manager); rc_override_active = hal.rcin->set_overrides(rc_override, 8); @@ -196,8 +171,10 @@ static void init_ardupilot() relay.init(); +#if MOUNT == ENABLED // initialise camera mount - camera_mount.init(); + camera_mount.init(serial_manager); +#endif /* setup the 'main loop is dead' check. Note that this relies on @@ -205,6 +182,8 @@ static void init_ardupilot() */ hal.scheduler->register_timer_failsafe(failsafe_check, 1000); + +#if CLI_ENABLED == ENABLED // If the switch is in 'menu' mode, run the main menu. // // Since we can't be sure that the setup or test mode won't leave @@ -213,12 +192,13 @@ static void init_ardupilot() // 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 startup_ground(); @@ -260,9 +240,9 @@ static void startup_ground(void) // initialise mission library mission.init(); - hal.uartA->set_blocking_writes(false); - hal.uartB->set_blocking_writes(false); - hal.uartC->set_blocking_writes(false); + // we don't want writes to the serial port to cause us to pause + // so set serial ports non-blocking once we are ready to drive + serial_manager.set_blocking_writes_all(false); gcs_send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to drive.")); } @@ -447,9 +427,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 }