Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Safe wrap timeouts in dshot #23312

Merged
merged 2 commits into from
Feb 28, 2024
Merged

Conversation

andyp1per
Copy link
Collaborator

@andyp1per andyp1per commented Mar 23, 2023

Refresh of #20430

This can safely use 32 bit timestamps in the rcout thread controlled via AP_RCOUT_USE_32BIT_TIME. The saves 100bytes of flash and about 0.7% CPU on an H7 which is fairly significant.

@andyp1per andyp1per force-pushed the pr-safe-wrap32 branch 4 times, most recently from 969e04f to f3c3fa1 Compare March 26, 2023 17:58
@andyp1per
Copy link
Collaborator Author

Without this I saw all-0 watchdogs and desyncs on my Chimera. My suspicion is that the 64bit maths burns too much CPU in the ISRs. I have tested on 4.3 with this and all flies fine. I highly recommend we backport to 4.3.

@tridge
Copy link
Contributor

tridge commented Apr 9, 2023

My suspicion is that the 64bit maths burns too much CPU in the ISRs.

that makes no sense at all. The ISRs happen at most at the dshot rate, so 1kHz or so. The 64 bit maths adds a few cycles (about 40ns per 64 bit add or subtract)). A watchdog requires 2s without a watchdog pat. There must be something else going on. To be spending that much time in ISRs would require an interrupt storm

@andyp1per
Copy link
Collaborator Author

andyp1per commented Apr 11, 2023

On the F427 the CPU cost is more significant - 1.5%. On my bdshot F427 quad this would have pushed it over the edge into poorer flight performance.

@andyp1per
Copy link
Collaborator Author

I'm flying this on all my copters without issue.

@andyp1per
Copy link
Collaborator Author

Have been flying with this for several months without issue. Since there are other places in the code where wraps can cause issues (e.g. ESC telemetry) and moving everything to 64bit is going to prove expensive, I would like to get this in - particularly the helper functions which I think enforces best practice as well as being much clearer to read in the code.

libraries/AP_HAL/system.h Show resolved Hide resolved
libraries/AP_HAL/system.h Outdated Show resolved Hide resolved
libraries/AP_HAL/system.h Outdated Show resolved Hide resolved
libraries/AP_HAL/system.h Outdated Show resolved Hide resolved
libraries/AP_HAL/system.h Outdated Show resolved Hide resolved
libraries/AP_HAL/system.h Outdated Show resolved Hide resolved
libraries/AP_HAL/system.h Outdated Show resolved Hide resolved
@tridge tridge removed the DevCallEU label Jun 21, 2023
@andyp1per andyp1per force-pushed the pr-safe-wrap32 branch 4 times, most recently from 18c9c70 to 3303393 Compare January 30, 2024 18:05
@andyp1per andyp1per requested a review from tridge January 30, 2024 19:30
@tridge tridge removed the DevCallEU label Jan 31, 2024
@andyp1per
Copy link
Collaborator Author

Type safe version. Verified that you cannot use different types for the time values.

32 bit version is 280 bytes smaller than 64 byte version

@tridge tridge removed the DevCallEU label Feb 21, 2024
@andyp1per
Copy link
Collaborator Author

You can see that godbolt doesn't find any significance

image

@andyp1per andyp1per force-pushed the pr-safe-wrap32 branch 2 times, most recently from f9f5a19 to 375caf8 Compare February 21, 2024 21:00
@andyp1per andyp1per requested a review from tridge February 21, 2024 21:03
@andyp1per
Copy link
Collaborator Author

On an F4 (MambaF405v2) 64-bit:

ThreadsV2
ISR           PRI=255 sp=0x20000000 STACK=1264/1536 LOAD= 5.1%
ArduCopter    PRI=182 sp=0x20000600 STACK=5584/7168 LOAD=29.1%
idle          PRI=  1 sp=0x200135F0 STACK= 296/ 504 LOAD=23.1%
UART_RX       PRI= 60 sp=0x2001D9D0 STACK= 880/1208 LOAD= 1.3%
OTG1          PRI= 60 sp=0x2001D068 STACK= 360/ 760 LOAD= 0.6%
monitor       PRI=183 sp=0x200108E0 STACK=1112/1464 LOAD= 0.6%
timer         PRI=181 sp=0x20011F20 STACK=1568/1976 LOAD= 1.8%
rcout         PRI=181 sp=0x20011500 STACK= 544/ 952 LOAD=17.7%
rcin          PRI=177 sp=0x20010EF0 STACK= 968/1464 LOAD= 1.4%
io            PRI= 58 sp=0x2000FED0 STACK=1552/2488 LOAD= 1.2%*
storage       PRI= 59 sp=0x20011910 STACK= 928/1464 LOAD= 0.6%
UART3         PRI= 60 sp=0x2001C738 STACK= 472/ 760 LOAD= 0.5%
UART6         PRI= 60 sp=0x2001BEC0 STACK= 472/ 760 LOAD= 0.5%
UART1         PRI= 60 sp=0x200165D0 STACK= 472/ 760 LOAD= 0.5%
log_io        PRI= 59 sp=0x20015D80 STACK=1360/2016 LOAD= 3.4%
OSD           PRI= 59 sp=0x200147A8 STACK= 984/1720 LOAD= 0.9%*
SPI1          PRI=181 sp=0x10002938 STACK= 848/1464 LOAD=10.7%
FTP           PRI= 58 sp=0x10005640 STACK=1056/3000 LOAD= 0.6%

32-bit:

ThreadsV2
ISR           PRI=255 sp=0x20000000 STACK=1280/1536 LOAD= 5.0%
ArduCopter    PRI=182 sp=0x20000600 STACK=5584/7168 LOAD=27.0%
idle          PRI=  1 sp=0x200135B0 STACK= 296/ 504 LOAD=28.7%
UART_RX       PRI= 60 sp=0x2001D9D0 STACK= 880/1208 LOAD= 1.4%
OTG1          PRI= 60 sp=0x2001D068 STACK= 360/ 760 LOAD= 0.6%
monitor       PRI=183 sp=0x200108A0 STACK=1112/1464 LOAD= 0.6%
timer         PRI=181 sp=0x20011EE0 STACK=1576/1976 LOAD= 1.7%
rcout         PRI=181 sp=0x200114C0 STACK= 552/ 952 LOAD=17.2%
rcin          PRI=177 sp=0x20010EB0 STACK= 968/1464 LOAD= 1.4%
io            PRI= 58 sp=0x2000FE90 STACK=1552/2488 LOAD= 1.2%*
storage       PRI= 59 sp=0x200118D0 STACK= 904/1464 LOAD= 0.7%
UART3         PRI= 60 sp=0x2001C738 STACK= 472/ 760 LOAD= 0.5%
UART6         PRI= 60 sp=0x2001BEC0 STACK= 472/ 760 LOAD= 0.5%
UART1         PRI= 60 sp=0x200165D0 STACK= 472/ 760 LOAD= 0.5%
log_io        PRI= 59 sp=0x20015D80 STACK=1592/2016 LOAD= 0.7%
OSD           PRI= 59 sp=0x200147A8 STACK= 984/1720 LOAD= 0.9%*
SPI1          PRI=181 sp=0x100028F8 STACK= 848/1464 LOAD=10.4%
FTP           PRI= 58 sp=0x10005600 STACK=1088/3000 LOAD= 0.5%

The 0.5% benefit is very consistent (so 2.8% benefit to the rcout thread), so a pretty decent bump for a change in type

libraries/AP_HAL/tests/test_timeouts.cpp Outdated Show resolved Hide resolved
add tests for timeout functions
… require it

correct timeout checking for dshot across timer wrap boundaries
fix trigger_groups timeout checks
use rcout_timer_t instead of uint32_t or uint64_t
@andyp1per andyp1per requested a review from tridge February 28, 2024 07:33
@peterbarker peterbarker merged commit b248ba5 into ArduPilot:master Feb 28, 2024
92 checks passed
@andyp1per andyp1per deleted the pr-safe-wrap32 branch February 28, 2024 11:26
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
Development

Successfully merging this pull request may close these issues.

3 participants