ardupilot icon indicating copy to clipboard operation
ardupilot copied to clipboard

HAL: Implement AP_HAL::loop_ms()

Open tridge opened this issue 3 years ago • 1 comments

This aims to reduce CPU load by replacing calls to AP_HAL::millis() with AP_HAL::loop_ms(). The millis() call is expensive (about 1.7us on H7), and we're making about 15k calls to it from the main thread per second. That is costing us about 2.5% of CPU, and also causing system timing jitter as it needs to take a system lock This should be considered very experimental

  • [ ] needs some checks in SITL to ensure loop_ms() isn't called from other than the main thread
  • [ ] add similar calls for micros() and micros64()

tridge avatar Feb 17 '22 05:02 tridge

Ok, I had to put system in RAM to get this to work on SPRacingH7:

diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/common_extf.ld b/libraries/AP_HAL_ChibiOS/hwdef/common/common_extf.ld
index 80b9b313c4..530aac4418 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/common_extf.ld
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/common_extf.ld
@@ -107,6 +107,7 @@ SECTIONS
         *libch.a:hal*.*(.text*)
         /* a selection of performance critical functions driven CPUInfo results */
         lib/lib*.a:Semaphores.*(.text*)
+        lib/lib*.a:system.*(.text*)
         lib/lib*.a:AP_Math.*(.text*)
         lib/lib*.a:vector3.*(.text*)
         lib/lib*.a:matrix3.*(.text*)
@@ -189,6 +190,7 @@ SECTIONS
         lib/lib*.a:AP_Math.*(.rodata*)
         lib/lib*.a:vector3.*(.rodata*)
         lib/lib*.a:matrix3.*(.rodata*)
+        lib/lib*.a:system.*(.rodata*)
         *libm.a:*(.rodata*)
         *(.ramdata*)
         . = ALIGN(4);

interestingly there is no discernible improvement in performance on this board

andyp1per avatar Feb 17 '22 11:02 andyp1per