diff --git a/Tools/AP_Periph/Web/scripts/pppgw_webui.lua b/Tools/AP_Periph/Web/scripts/pppgw_webui.lua index 9c458c7e079c8..58cf3590bad51 100644 --- a/Tools/AP_Periph/Web/scripts/pppgw_webui.lua +++ b/Tools/AP_Periph/Web/scripts/pppgw_webui.lua @@ -221,6 +221,7 @@ local DYNAMIC_PAGES = { IP Netmask Gateway + MCU Temperature ]] } diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp index d0fedce547915..da4ae589df661 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp @@ -299,6 +299,7 @@ void UARTDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS) _rx_initialised = false; _readbuf.set_size_best(rxS); } + _rts_threshold = _readbuf.get_size() / 16U; bool clear_buffers = false; if (b != 0) { @@ -1414,10 +1415,10 @@ __RAMFUNC__ void UARTDriver::update_rts_line(void) return; } uint16_t space = _readbuf.space(); - if (_rts_is_active && space < 16) { + if (_rts_is_active && space < _rts_threshold) { _rts_is_active = false; palSetLine(arts_line); - } else if (!_rts_is_active && space > 32) { + } else if (!_rts_is_active && space > _rts_threshold+16) { _rts_is_active = true; palClearLine(arts_line); } diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.h b/libraries/AP_HAL_ChibiOS/UARTDriver.h index e50230d427ae6..f21c1d7aa8be4 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.h +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.h @@ -182,6 +182,7 @@ class ChibiOS::UARTDriver : public AP_HAL::UARTDriver { #endif ByteBuffer _readbuf{0}; ByteBuffer _writebuf{0}; + uint32_t _rts_threshold; HAL_Semaphore _write_mutex; #ifndef HAL_UART_NODMA const stm32_dma_stream_t* rxdma; diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrangePlus/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrangePlus/hwdef.dat index 236b2b6dcbd57..d2163dbf14520 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrangePlus/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrangePlus/hwdef.dat @@ -99,3 +99,5 @@ BOARD_VALIDATE $CHECK_IMU0_PRESENT $CHECK_IMU1_PRESENT $CHECK_IMU2_PRESENT $CHEC # build ABIN for flash-from-bootloader support: env BUILD_ABIN True define HAL_INS_HIGHRES_SAMPLE 6 + +define AP_NETWORKING_BACKEND_PPP 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubePilot-PPPGW/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubePilot-PPPGW/hwdef.dat index 70c50244fd8ef..bf18499b1e0c5 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubePilot-PPPGW/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubePilot-PPPGW/hwdef.dat @@ -1,17 +1,24 @@ include ../CubePilot-CANMod/hwdef.dat # we need RTS/CTS for the PPP link -undef PD14 -undef PD13 undef PE0 undef PE1 # need to use UART8 to get RTS/CTS PE1 UART8_TX UART8 PE0 UART8_RX UART8 -PD13 UART8_RTS UART8 -PD14 UART8_CTS UART8 +PA10 UART8_RTS UART8 +PC11 UART8_CTS_GPIO UART8 SERIAL_ORDER OTG1 UART8 +PA4 VDD_5V_SENS ADC1 SCALE(2) + +undef HAL_USE_ADC +define HAL_USE_ADC TRUE +define HAL_WITH_MCU_MONITORING 1 + +# can optionally run at half clock +# MCU_CLOCKRATE_MHZ 200 + include ../include/network_PPPGW.inc diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h index 3abf537e704c3..e615ce33ab6ea 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h @@ -202,6 +202,9 @@ #if HAL_CUSTOM_MCU_CLOCKRATE == 480000000 #define STM32_PLL1_DIVN_VALUE 120 #define STM32_PLL1_DIVQ_VALUE 12 +#elif HAL_CUSTOM_MCU_CLOCKRATE == 200000000 +#define STM32_PLL1_DIVN_VALUE 50 +#define STM32_PLL1_DIVQ_VALUE 5 #else #error "Unable to configure custom clockrate" #endif diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index 9b3884f229d7a..fedc7f84c0f52 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -189,9 +189,9 @@ def get_alt_function(self, mcu, pin, function): return 0 return None - if function and function.endswith("_RTS") and ( + if function and (function.endswith("_RTS") or function.endswith("_CTS_GPIO")) and ( function.startswith('USART') or function.startswith('UART')): - # we do software RTS + # we do software RTS and can do either software CTS or hardware CTS return None for label in self.af_labels: @@ -1891,6 +1891,8 @@ def write_UART_config(self, f): rts_line_name = dev + '_RTS' rts_line = self.make_line(rts_line_name) cts_line = self.make_line(dev + '_CTS') + if cts_line == "0": + cts_line = self.make_line(dev + '_CTS_GPIO') if rts_line != "0": have_rts_cts = True f.write('#define HAL_HAVE_RTSCTS_SERIAL%u\n' % num) @@ -2927,12 +2929,12 @@ def valid_type(self, ptype, label): if ptype == 'OUTPUT' and re.match(r'US?ART\d+_(TXINV|RXINV)', label): return True m1 = re.match(r'USART(\d+)', ptype) - m2 = re.match(r'USART(\d+)_(RX|TX|CTS|RTS)', label) + m2 = re.match(r'USART(\d+)_(RX|TX|CTS|RTS|CTS_GPIO)', label) if (m1 and not m2) or (m2 and not m1) or (m1 and m1.group(1) != m2.group(1)): '''usart numbers need to match''' return False m1 = re.match(r'UART(\d+)', ptype) - m2 = re.match(r'UART(\d+)_(RX|TX|CTS|RTS)', label) + m2 = re.match(r'UART(\d+)_(RX|TX|CTS|RTS|CTS_GPIO)', label) if (m1 and not m2) or (m2 and not m1) or (m1 and m1.group(1) != m2.group(1)): '''uart numbers need to match''' return False diff --git a/libraries/AP_Networking/AP_Networking.cpp b/libraries/AP_Networking/AP_Networking.cpp index 63bdb7235af9b..1d2edb1c98aa0 100644 --- a/libraries/AP_Networking/AP_Networking.cpp +++ b/libraries/AP_Networking/AP_Networking.cpp @@ -76,7 +76,7 @@ const AP_Param::GroupInfo AP_Networking::var_info[] = { // @Param: TESTS // @DisplayName: Test enable flags // @Description: Enable/Disable networking tests - // @Bitmask: 0:UDP echo test,1:TCP echo test, 2:TCP discard test + // @Bitmask: 0:UDP echo test,1:TCP echo test, 2:TCP discard test, 3:TCP reflect test // @RebootRequired: True // @User: Advanced AP_GROUPINFO("TESTS", 7, AP_Networking, param.tests, 0), diff --git a/libraries/AP_Networking/AP_Networking.h b/libraries/AP_Networking/AP_Networking.h index eaa51ffc8e64e..ea94afe8e6a52 100644 --- a/libraries/AP_Networking/AP_Networking.h +++ b/libraries/AP_Networking/AP_Networking.h @@ -283,11 +283,13 @@ class AP_Networking TEST_UDP_CLIENT = (1U<<0), TEST_TCP_CLIENT = (1U<<1), TEST_TCP_DISCARD = (1U<<2), + TEST_TCP_REFLECT = (1U<<3), }; void start_tests(void); void test_UDP_client(void); void test_TCP_client(void); void test_TCP_discard(void); + void test_TCP_reflect(void); #endif // AP_NETWORKING_TESTS_ENABLED #if AP_NETWORKING_REGISTER_PORT_ENABLED diff --git a/libraries/AP_Networking/AP_Networking_tests.cpp b/libraries/AP_Networking/AP_Networking_tests.cpp index 4747b0ce8873e..8a87ed15ba388 100644 --- a/libraries/AP_Networking/AP_Networking_tests.cpp +++ b/libraries/AP_Networking/AP_Networking_tests.cpp @@ -33,6 +33,11 @@ void AP_Networking::start_tests(void) "TCP_discard", 8192, AP_HAL::Scheduler::PRIORITY_UART, -1); } + if (param.tests & TEST_TCP_REFLECT) { + hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Networking::test_TCP_reflect, void), + "TCP_reflect", + 8192, AP_HAL::Scheduler::PRIORITY_UART, -1); + } } /* @@ -140,4 +145,59 @@ void AP_Networking::test_TCP_discard(void) } } +/* + start TCP reflect (TCP echo throughput) test + */ +void AP_Networking::test_TCP_reflect(void) +{ + startup_wait(); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "TCP_reflect: starting"); + const char *dest = param.test_ipaddr.get_str(); + auto *sock = new SocketAPM(false); + if (sock == nullptr) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "TCP_reflect: failed to create socket"); + return; + } + // connect to the echo service, which is port 7 + while (!sock->connect(dest, 7)) { + hal.scheduler->delay(10); + } + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "TCP_reflect: connected"); + const uint32_t bufsize = 4096; + uint8_t *buf = (uint8_t*)malloc(bufsize); + for (uint32_t i=0; idelay(1); + continue; + } + const bool will_send = total_sent < total_recv + max_disparity; + if (will_send) { + total_sent += sock->send(buf, bufsize); + } + if (sock->pollin(0)) { + uint32_t n = sock->recv(buf, bufsize, 0); + if (n == 0 && !will_send) { + hal.scheduler->delay_microseconds(100); + } + total_recv += n; + } + const uint32_t now = AP_HAL::millis(); + + if (now - last_report_ms >= 1000) { + float dt = (now - last_report_ms)*0.001; + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Reflect throughput %.3f kbyte/sec (disparity %u)", ((total_recv-last_recv)/dt)*1.0e-3, unsigned(total_sent-total_recv)); + last_recv = total_recv; + last_report_ms = now; + } + } +} + #endif // AP_NETWORKING_ENABLED && AP_NETWORKING_TESTS_ENABLED diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index 7171b241c5296..619aeb524ff82 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -1650,6 +1650,10 @@ function Motors_dynamic:init(expected_num_motors) end -- desc analog = {} +-- return MCU temperature in degrees C +---@return number -- MCU temperature +function analog:mcu_temperature() end + -- desc ---@return AP_HAL__AnalogSource_ud|nil function analog:channel() end diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 40910361296c5..a0c8bb7faddb1 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -622,6 +622,8 @@ singleton hal.analogin depends !defined(HAL_DISABLE_ADC_DRIVER) singleton hal.analogin rename analog singleton hal.analogin literal singleton hal.analogin method channel AP_HAL::AnalogSource ANALOG_INPUT_NONE'literal +singleton hal.analogin method mcu_temperature float +singleton hal.analogin method mcu_temperature depends HAL_WITH_MCU_MONITORING include AP_Motors/AP_MotorsMatrix_Scripting_Dynamic.h depends APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI singleton AP_MotorsMatrix_Scripting_Dynamic depends APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI diff --git a/libraries/AP_SerialManager/AP_SerialManager.cpp b/libraries/AP_SerialManager/AP_SerialManager.cpp index 1ff5dae93df6c..9818738b3fb90 100644 --- a/libraries/AP_SerialManager/AP_SerialManager.cpp +++ b/libraries/AP_SerialManager/AP_SerialManager.cpp @@ -183,7 +183,7 @@ const AP_Param::GroupInfo AP_SerialManager::var_info[] = { // @Param: 1_PROTOCOL // @DisplayName: Telem1 protocol selection // @Description: Control what protocol to use on the Telem1 port. Note that the Frsky options require external converter hardware. See the wiki for details. - // @Values: -1:None, 1:MAVLink1, 2:MAVLink2, 3:Frsky D, 4:Frsky SPort, 5:GPS, 7:Alexmos Gimbal Serial, 8:Gimbal, 9:Rangefinder, 10:FrSky SPort Passthrough (OpenTX), 11:Lidar360, 13:Beacon, 14:Volz servo out, 15:SBus servo out, 16:ESC Telemetry, 17:Devo Telemetry, 18:OpticalFlow, 19:RobotisServo, 20:NMEA Output, 21:WindVane, 22:SLCAN, 23:RCIN, 24:EFI Serial, 25:LTM, 26:RunCam, 27:HottTelem, 28:Scripting, 29:Crossfire VTX, 30:Generator, 31:Winch, 32:MSP, 33:DJI FPV, 34:AirSpeed, 35:ADSB, 36:AHRS, 37:SmartAudio, 38:FETtecOneWire, 39:Torqeedo, 40:AIS, 41:CoDevESC, 42:DisplayPort, 43:MAVLink High Latency, 44:IRC Tramp, 45:DDS XRCE, 46:IMUDATA + // @Values: -1:None, 1:MAVLink1, 2:MAVLink2, 3:Frsky D, 4:Frsky SPort, 5:GPS, 7:Alexmos Gimbal Serial, 8:Gimbal, 9:Rangefinder, 10:FrSky SPort Passthrough (OpenTX), 11:Lidar360, 13:Beacon, 14:Volz servo out, 15:SBus servo out, 16:ESC Telemetry, 17:Devo Telemetry, 18:OpticalFlow, 19:RobotisServo, 20:NMEA Output, 21:WindVane, 22:SLCAN, 23:RCIN, 24:EFI Serial, 25:LTM, 26:RunCam, 27:HottTelem, 28:Scripting, 29:Crossfire VTX, 30:Generator, 31:Winch, 32:MSP, 33:DJI FPV, 34:AirSpeed, 35:ADSB, 36:AHRS, 37:SmartAudio, 38:FETtecOneWire, 39:Torqeedo, 40:AIS, 41:CoDevESC, 42:DisplayPort, 43:MAVLink High Latency, 44:IRC Tramp, 45:DDS XRCE, 46:IMUDATA, 48:PPP // @User: Standard // @RebootRequired: True AP_GROUPINFO("1_PROTOCOL", 1, AP_SerialManager, state[1].protocol, DEFAULT_SERIAL1_PROTOCOL),