diff --git a/libraries/AP_BoardConfig/AP_BoardConfig.cpp b/libraries/AP_BoardConfig/AP_BoardConfig.cpp index d3a10010e6..68fb90eb2d 100644 --- a/libraries/AP_BoardConfig/AP_BoardConfig.cpp +++ b/libraries/AP_BoardConfig/AP_BoardConfig.cpp @@ -88,21 +88,55 @@ const AP_Param::GroupInfo AP_BoardConfig::var_info[] = { AP_GROUPINFO("PWM_COUNT", 0, AP_BoardConfig, pwm_count, BOARD_PWM_COUNT_DEFAULT), #if AP_FEATURE_RTSCTS +#ifdef HAL_HAVE_RTSCTS_SERIAL1 // @Param: SER1_RTSCTS // @DisplayName: Serial 1 flow control // @Description: Enable flow control on serial 1 (telemetry 1) on Pixhawk. You must have the RTS and CTS pins connected to your radio. The standard DF13 6 pin connector for a 3DR radio does have those pins connected. If this is set to 2 then flow control will be auto-detected by checking for the output buffer filling on startup. Note that the PX4v1 does not have hardware flow control pins on this port, so you should leave this disabled. // @Values: 0:Disabled,1:Enabled,2:Auto // @RebootRequired: True // @User: Advanced - AP_GROUPINFO("SER1_RTSCTS", 1, AP_BoardConfig, state.ser1_rtscts, BOARD_SER1_RTSCTS_DEFAULT), + AP_GROUPINFO("SER1_RTSCTS", 1, AP_BoardConfig, state.ser_rtscts[1], BOARD_SER1_RTSCTS_DEFAULT), +#endif +#ifdef HAL_HAVE_RTSCTS_SERIAL2 // @Param: SER2_RTSCTS // @DisplayName: Serial 2 flow control // @Description: Enable flow control on serial 2 (telemetry 2) on Pixhawk and STATE. You must have the RTS and CTS pins connected to your radio. The standard DF13 6 pin connector for a 3DR radio does have those pins connected. If this is set to 2 then flow control will be auto-detected by checking for the output buffer filling on startup. // @Values: 0:Disabled,1:Enabled,2:Auto // @RebootRequired: True // @User: Advanced - AP_GROUPINFO("SER2_RTSCTS", 2, AP_BoardConfig, state.ser2_rtscts, 2), + AP_GROUPINFO("SER2_RTSCTS", 2, AP_BoardConfig, state.ser_rtscts[2], 2), +#endif + +#ifdef HAL_HAVE_RTSCTS_SERIAL3 + // @Param: SER3_RTSCTS + // @DisplayName: Serial 3 flow control + // @Description: Enable flow control on serial 3. You must have the RTS and CTS pins connected to your radio. The standard DF13 6 pin connector for a 3DR radio does have those pins connected. If this is set to 2 then flow control will be auto-detected by checking for the output buffer filling on startup. + // @Values: 0:Disabled,1:Enabled,2:Auto + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("SER3_RTSCTS", 23, AP_BoardConfig, state.ser_rtscts[3], 2), +#endif + +#ifdef HAL_HAVE_RTSCTS_SERIAL4 + // @Param: SER4_RTSCTS + // @DisplayName: Serial 4 flow control + // @Description: Enable flow control on serial 4. You must have the RTS and CTS pins connected to your radio. The standard DF13 6 pin connector for a 3DR radio does have those pins connected. If this is set to 2 then flow control will be auto-detected by checking for the output buffer filling on startup. + // @Values: 0:Disabled,1:Enabled,2:Auto + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("SER4_RTSCTS", 24, AP_BoardConfig, state.ser_rtscts[4], 2), +#endif + +#ifdef HAL_HAVE_RTSCTS_SERIAL5 + // @Param: SER5_RTSCTS + // @DisplayName: Serial 5 flow control + // @Description: Enable flow control on serial 5. You must have the RTS and CTS pins connected to your radio. The standard DF13 6 pin connector for a 3DR radio does have those pins connected. If this is set to 2 then flow control will be auto-detected by checking for the output buffer filling on startup. + // @Values: 0:Disabled,1:Enabled,2:Auto + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("SER5_RTSCTS", 25, AP_BoardConfig, state.ser_rtscts[5], 2), +#endif #endif #if HAL_HAVE_SAFETY_SWITCH diff --git a/libraries/AP_BoardConfig/AP_BoardConfig.h b/libraries/AP_BoardConfig/AP_BoardConfig.h index 11c8f01855..62b2720a11 100644 --- a/libraries/AP_BoardConfig/AP_BoardConfig.h +++ b/libraries/AP_BoardConfig/AP_BoardConfig.h @@ -190,8 +190,7 @@ private: AP_Int16 safety_option; AP_Int32 ignore_safety_channels; #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS - AP_Int8 ser1_rtscts; - AP_Int8 ser2_rtscts; + AP_Int8 ser_rtscts[6]; AP_Int8 sbus_out_rate; #endif AP_Int8 board_type; diff --git a/libraries/AP_BoardConfig/board_drivers.cpp b/libraries/AP_BoardConfig/board_drivers.cpp index c0152897ea..6a5a4b874a 100644 --- a/libraries/AP_BoardConfig/board_drivers.cpp +++ b/libraries/AP_BoardConfig/board_drivers.cpp @@ -410,11 +410,32 @@ void AP_BoardConfig::board_autodetect(void) void AP_BoardConfig::board_setup_uart() { #if AP_FEATURE_RTSCTS - hal.uartC->set_flow_control((AP_HAL::UARTDriver::flow_control)state.ser1_rtscts.get()); - if (hal.uartD != nullptr) { - hal.uartD->set_flow_control((AP_HAL::UARTDriver::flow_control)state.ser2_rtscts.get()); +#ifdef HAL_HAVE_RTSCTS_SERIAL1 + if (hal.uartC != nullptr) { + hal.uartC->set_flow_control((AP_HAL::UARTDriver::flow_control)state.ser_rtscts[1].get()); } #endif +#ifdef HAL_HAVE_RTSCTS_SERIAL2 + if (hal.uartD != nullptr) { + hal.uartD->set_flow_control((AP_HAL::UARTDriver::flow_control)state.ser_rtscts[2].get()); + } +#endif +#ifdef HAL_HAVE_RTSCTS_SERIAL3 + if (hal.uartB != nullptr) { + hal.uartB->set_flow_control((AP_HAL::UARTDriver::flow_control)state.ser_rtscts[3].get()); + } +#endif +#ifdef HAL_HAVE_RTSCTS_SERIAL4 + if (hal.uartE != nullptr) { + hal.uartE->set_flow_control((AP_HAL::UARTDriver::flow_control)state.ser_rtscts[4].get()); + } +#endif +#ifdef HAL_HAVE_RTSCTS_SERIAL5 + if (hal.uartF != nullptr) { + hal.uartF->set_flow_control((AP_HAL::UARTDriver::flow_control)state.ser_rtscts[5].get()); + } +#endif +#endif } /*